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arbitrary  rigid  polyhedral  moving  object  P  with  three  translational  and  three 
rotational  degrees  of  freedom,  find  a  continuous,  collision^free  path  taking  "P" 
from  some  initial  configuration  to  a  desired  goal  configuration. 


This  thesis  describes  the  first  known  implementation  of  a  complete  algorithm  (at 
a  given  resolution)  for  the  full  six  degree  of  freedom  Movers'  problem.  The  al¬ 
gorithm  transforms  the  six  degree  of  freedom  planning  problem  into  a  point  navigation 
problem  in  a  six-dimensional  configuration  space  (called  C-Space) .  The  C-Space 
obstacles,  which  characterize  the  physically  unachievable  configurations,  are 
directly  represented  by  six-dimensional  manifolds  whose  boundaries  are  five 
dimensional  C-surfaces.  By  characterizing  these  surfaces  and  their  intersections, 
collision-free  paths  may  be  found  by  the  closure  of  three  operators  which 

(i)  slide  along  5-dimensional  level  C-surfaces  parallel  to  C-Space  obstacles; 

(ii)  slide  along  1-  to  4-dimenslonal  intersections  of  level  C-surfaces;  and 

(iii)  jump  between  6-dimensional  obstacles. 

Y 

Implementing  the  point  navigation  operators  requires  solving  fundamental  repre¬ 
sentational  and  algorithmic  questions:  we  will  derive  new  structural  properties 
of  the  C-Space  constraints  and  show  how  to  construct  and  represent  C-surfaces 
and  their  intersection  manifolds.  A  definition  and  new  theoretical  results 
are  presented  for  a  six-dimensional  C-Space  extension  of  the  generalized 
Voronoi  diagram;  called  the  "C-Voronoi  diagram",  whose  structure  we  relate  to 
the  C-surface  intersection  manifolds.  The  representations  and  algorithms  we 
develop  impact  many  geometric  planning  problems,  and  extend  to  Cartesian 
manipulators  with  six  degrees  of  freedom. 
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Motion  Planning  with  Six  Degrees  of  Freedom 

by 

Bruce  Randall  Donald 

Abstract:  The  motion  planning  problem  is  of  central  importance  to  the  fields 
of  robotics,  spatial  planning,  and  automated  design.  In  robotics  we  are  interested 
in  the  automatic  synthesis  of  robot  motions,  given  high-level  specifications  of 
tasks  and  geometric  models  of  the  robot  and  obstacles.  The  Mover’s  problem 
is  to  find  a  continuous,  collision-free  path  for  a  moving  object  through  an 
environment  containing  obstacles.  We  present  an  implemented  algorithm  for  the 
classical  formulation  of  the  three-dimensional  Movers’  problem:  Given  an  arbitrary 
rigid  polyhedral  moving  object  P  with  three  translational  and  three  rotational 
degrees  of  freedom,  find  a  continuous,  collision-free  path  taking  P  from  some  initial 
configuration  t  >  a  desired  goal  configuration. 

This  thesis  describes  the  first  known  implementation  of  a  complete  algorithm 
(at  a  given  resolution)  for  the  full  six  degree  of  freedom  Movers’  problem.  The 
algorithm  transforms  the  six  degree  of  freedom  planning  problem  into  a  point 
navigation  problem  in  a  six-dimensional  configuration  space  (called  C-Spuce).  The 
C-Space  obstacles,  which  characterize  the  physically  unachievable  configurations, 
are  directly  represented  by  six-dimensional  manifolds  whose  boundaries  are  five 
dimensional  C-surfaces.  By  characterizing  these  surfaces  and  their  intersections, 
collision-free  paths  may  be  found  by  the  closure  of  three  operators  which  (i) 
slide  along  o-dimensional  level  C-surfaces  parallel  to  C-Space  obstacles;  (ii)  slide 
along  1-  to  '1-dimensional  intersections  of  level  C-surfaces;  and  (iii)  jump  between 
6-diincnsional  obstacles. 

Implementing  the  point  navigation  operators  requires  solving  fundamental 
representational  and  algorithmic  questions:  we  will  derive  now  structural  properties 
of  the  C-Spacc  constraints  and  show  how  to  construct  and  represent  C-surfaces  and 
their  intersection  manifolds.  A  definition  and  now  theoretical  results  arc  presented 
for  a  six-dimensional  C-Space  extension  of  the  generalized  Voronoi  diagram,  called 
the  C-  Voronoi  diagram,  whose  structure  we  relate  to  the  C-surf  acc  intersection 
manifolds.  The  representations  and  algorithms  we  develop  impact  many  geometric 
planning  problems,  and  extend  to  Cartesian  manipulators  with  six  degrees  of 
freedom. 
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Geometric  Planning  Problems 

Introduction  and  Statement  of  the  Problem 

The  motion  planning  problem  is  of  central  importance  to  the  fields  of  robotics, 
spatial  planning,  and  automated  design.  In  robotics  we  arc  interested  in  the 
automatic  synthesis  of  robot  motions,  given  high-level  specifications  of  tasks  and 
geometric  models  of  the  robot  and  obstacles.  The  problem  is  to  find  a  continuous, 
collision-free  path  for  a  moving  object,  through  an  environment  containing  obstacles; 
hence  it  has  also  been  called  the  Find-Path  or  Piano  Movers'  problem.  In  its  most 
general  formulation  the  object  can  have  an  arbitrary  number  of  hinges  and  joints, 
and  in  some  cases  coordinated  motion  planning  for  multiple  objects  has  been 
considered.  We  will  confine  ourselves  to  the  classical  formulation  of  the  Movers’ 
problem:  Given  an  arbitrary  rigid  polyhedral  moving  object  P,  find  a  continuous, 
collision-free  path  taking  P  from  some  initial  configuration  to  a  desired  goal 
configuration.  Wc  are  particularly  interested  in  the  3-ditnensional  Movers’  problem, 
for  an  object  with  3  translational  and  3  rotational  degrees  of  freedom.  This  thesis 
describes  the  first  known  implementation  of  a  complete  algorithm  (at  a  given 
resolution)  for  the  full  G  degree  of  freedom  Movers’  problem. 

1.1.  What  are  Geometric  Planning  Problems? 

Our  work  has  impact  on  a  class  of  geometric  planning  problems.  In  robotics 
wo  arc  typically  interested  in  motion  planning  for  a  mobile  robot  or  manipluator. 
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Figure  1.1.  An  example  of  a  solution  path  for  the  classical  Mover’s  problem  with  six  degrees 
of  freedom.  This  illustration  is  a  “tiinc-lapsc”  picture  of  a  path  found  by  our  planner  for  a 
'  aminer-shaped  object.  In  all  our  examples,  the  workspace  is  bounded  by  a  box  (which  is  not 
shown).  This  solution  path  requires  use  of  all  three  rotational  degrees  of  freedom. 

In  Computer- Aide'’  Design  (CAD),  the  problem  ol’  automated  structural  design 
for  n  structural  members  is  also  an  instance  of  the  most  general  form  of  the 
Mover’s  problem.  The  problem  of  determining  whether  an  object  can  be  assembled 
as  designed,  and  of  generating  an  assembly  plan  if  it  can,  is  also  in  this  class. 
Examples  of  geometric  planning  problems  include: 

(i)  The  Find-Path  or  Movers  ’problem  is  to  find  continuous,  collision-free  path 
for  one  or  more  moving  objects  in  the  presence  of  obstacles.  Find-path  problems 
fall  into  two  broad  categories:  single-body  and  multiple-body  motion  planning 
problems.  In  the  classical  Movers’  problem,  a  single  rigid  polyhedral  object  must 
be  moved  through  a  workspace  containing  polyhedral  obstacles.  For  the  linked 
or  hinged  body  Movers’  problem,  a  set  of  moving  objects  connected  via  joints  or 
linkages  must  be  moved.  An  industrial  robot  arm  is  a  typical  example  of  such  an 


Figure  1.2.  A  different  view  or  ttic  solution  pa'h  for  the  hammer  example,  with  the  obstacles 
“transparent"  to  allow  us  to  view  the  rotations  bettor. 

object.  In  the  coordinated  planning  problem,  a  number  of  independent  (i.e.,  not 
necessarily  linked)  objects  must  be  moved.  An  algorithm  for  mi  duple  body  motion 
planning  must  ensure  that,  the  moving  objects  collide  neither  with  the  walls  nor 
with  each  other. 

(ii)  The  find-space  problem  is  to  find  a  collision-free  placement  for  one  or 
more  objects  in  a  field  of  obstacles.  By  analogy  with  the  find-path  problem,  we 
can  speak  of  the  classical,  linked-body,  and  coordinated  find-space  problems.  In 
cornpuuer-aided  design  and  automated  design,  the  find-space  problem  is  typically 
subject  to  additional  geometric  constraints.  Lozano-Pcrcz  (1983)  grouped  find-path 
and  find-space  algorithms  together  as  the  spatial  planning  problems. 

(iii)  The  /ine-rrioiionproblcm  entails  motion-planning  along  obstacle  surfaces, 
typically  while  maintaining  some  applied  force.  Collision-free  paths  and  placements 
avoid  obstacles:  however,  for  many  tasks  in  robotics  and  in  automated  design,  it 


Figure  1.3.  Application  Example:  Planning  for  an  industrial  robot  arm  with  nix  degrees  of 
freedom  is  an  example  of  the  Imhod-body  movers’  problem.  (Figure  Courtesy  of  Rodney  brooks). 

is  necessary  to  plan  motions  and  placements  in  contact  with  obstacle  surfaces.  For 
example,  consider  the  tasks  of  welding,  insertion,  and  assembly  in  robotics.  These 


tasks  require  compliant  motions,  entailing  consideration  of  additional  physical 
constraints  such  as  friction,  kinematics,  and  force  control.  However,  the  compliant 
motion  planning  problem  has  a  strong  geometric  flavor  and  its  solution  requires 
the  tools  of  spatial  planning  (see  Mason  (1981),  Erdmann  (1984)). 

(iv)  Recently,  researchers  have  begun  to  consider  motion  planning  with 
uncertainly  (Mason  (1981),  Ilrooks  (1982),  Lozano-Pcrcz,  Mason,  and  Taylor  (1983), 
Erdmann  (1984)).  Droadly  speaking,  uncertainty  may  arise  from  inaccuracy  in 


object  models,  sensors,  or  control.  Motion  planning  with  uncertainty  also  presumes 
algorithms  and  representations  from  spatial  planning. 

As  wc  can  see,  all  geometric  planning  problems  contain  components  of  the 
spatial  planning  problem,  especially  if  the  underlying  geometries  are  the  same.  In 


(mu  n*n 
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Figure  1.4.  Example  from  computer-aided  design:  Automatically  generated  flat-plate  structure 
from  Donald  (10831*).  How  can  we  generate  structural  patterns  subject  to  the  constraints  of  the 
building  envelope  and  mechanical  coie? 


particular,  for  high-dimensional  configuration  spaces,  the  theoretical  analyses  of 
Mason  (1981),  Lozano-Perez,  Mason,  and  Taylor  (1983),  and  Erdmann  (1984)  all 
presume  geometric  results  which  are  derived  in  this  thesis. 

This  work  impacts  all  geometric  planning  problems.  To  illustrate  the  theoretical 
results,  we  address  one  particular  problem,  namely  the  classical  Movers’  problem 
with  six  degrees  of  freedom.  Our  algorithms  immediately  generalize  to  applications 
involving  gross-motion  and  Gne-motion  planning  for  Cartesian  manipulators  with 
six  degrees  of  freedom. 
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Figure  1.14.  A  cli(Ti-roiiL  view  of  the  same  solution  path,  showing  how  the  1, -shaped  object  must 
rotate  U>  attain  the  final  position.  The  first  six  frames  arc  not  shown,  since  the  moving  is  not 
visible  from  this  perspective.  Solution  Path  l  (view  2);  frames  7  15 


Figure  1.16.  Solution  ttith  1  (view  2);  friime.s  25  33 


Figure  1.21.  A  detail  of  the  path  for  the  L-Shapcd  example.  The  detail  »  in  "time-lapse”  format, 
and  shows  a  complex  double  rotation  near  the  goal  configuration. 

Examples  of  "classical”  find-path  problems  solved  by  our  planner  niay  be  found 
throughout  thh  chap'er,  and  also  at  the  end  of  chapter  2  (section  2.4,  “ Examples 
of  the  Local  Experts  in  Use").  See  fig.  1.5-21,  1.22-28,  and  2.7-21.  In  general, 
geometric  planning  problems  with  more  than  three  degrees  of  freedom  have  proven 
extremely  difficult  to  solve.  We  believe  that  in  part,  this  difficulty  has  been  due 
to  the  unresolved  issues  in  the  mathematics  of  spatial  planning.  By  solving  these 
problems  for  the  six  degree  of  freedom  case,  and  illustrating  the  results  for  the 
find-path  problem  (which  holds  considerable  intrinsic  interest),  we  hope  to  provide 
a  geometric  foundation  which  will  make  all  geometric  planning  problems  feasible. 
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1.2.  A  Simple  Example:  How  to  Find  a  Path  for  a  Point  Amidst  3-D 
Polyhedral  Obstacles 


We  will  begin  by  discussing  an  algorithm  for  navigating  a  single  point  amidst 
polyhedral  obstacles  in  three-dimension  Euclidean  space  Wc  then  review  the 
configuration  space  transformation  of  Lozano-Perez  (11)83),  which  transforms  the 
problem  of  reasoning  about  the  motion  of  a  polyhedral  object  to  reasoning  about  a 
single  point  in  configuration  space.  If  the  configuration  space  is  isomorphic  to  Ut3, 
then  the  point  navigation  algorithm  can  be  applied  directly  to  find  collision  free 
paths.  In  this  thesis  wc  will  generalize  the  point  navigation  algorithm  to  work  in 
the  configuration  space  for  a  three  dimensional  polyhedral  object  with  six  degrees 
of  freedom. 

The  six  degree  of  freedom  planner  is  based  on  the  following  analogy:  suppose 
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Figure  1.24.  Punk-Problem,  frames  (7-12),  view  1 
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Figure  1. 27.  I’uiile-Froblein,  frames  (1-6),  view  3 
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we  wish  to  find  a  path  for  a  point  in  9?'*,  avoiding  collisions  with  polyhedral 
obstacles,  where  each  polyhedron  is  modeled  as  the  intersection  of  a  finite  number 
of  half-spaces  of  9?,!.  One  solution  might  be  to  move  until  the  point  comes  in 
contact  with  a  polyhedron,  and  then  to  move  around  the  obstacle  by  traversing  the 
edge-graph  on  its  boundary.  (Refer  to  figure  1.39,  ignoring  the  caption  for  now). 
Each  arc  in  the  edge-graph  is  the  intersection  of  two  surfaces  bounding  half-spaces. 
Even  if  the  polyhedra  are  allowed  to  overlap,  the  technique  will  still  work  since  their 
intersections  have  the  same  structure.  Naturally,  we  will  also  need  some  technique 
for  jumping  from  one  obstacle  to  another. 

To  summarize,  we  can  find  a  collision-free  path  for  a  point  amidst  obstacle 
polyhedra  in  9?1'  through  the  closure  of  three  operators: 


The  Point  Navigation  Operators 


Operator  (i)  slides  along  the  1-dimcnsiona!  edges,  which  lie  in  the  intersections 
of  the  obstacle  planes; 

Operator  (ii)  slides  along  the  two  dimensional  obstacle  planes,  which  contain 
faces  of  the  obstacle  poljdiedra; 

Operator  (iii)  jumps  from  one  3- dimensional  obstacle  to  another. 


We  now  review  the  configuration  space  transformation.  Using  this  transfor- 
in  itr,  simplest  form,  the  find  path,  problem  in  three  dimensions  without 
rotations  is  reduced  to  the  point  navigation  problem  amidst  polyhedral  obstacles. 
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1.3.  Configuration  Space 

The  configuration  of  a  moving  object  is  a  vector  of  parameters  representing 
its  combined  translation  and  orientation,  relative  to  a  specified  coordinate  system. 
For  the  classical  Movers’  problem  in  the  plane,  a  typical  configuration 


represents  a  displacement  (translation)  of  (x,y),  and  a  rotation  by  0.  (l'or  example, 
imagine  a  polygon  displaced  by  ( x.y ),  and  rotated  by  0  about  one  of  its  vertices). 
For  the  six  degree  of  freedom  classical  Movers’  problem,  a  typical  conDguration 

X  =  ( x,y ,  2,  £(0)) 

represents  a  displacement  (translation)  of  ( x,y ,  a),  and  a  three  dimensional  rotation 
£(©).  The  three  dimensional  rotation  group  is  a  three  parameter  family;  typical 
representations  of  rotations  include  liulcr  angles,  (Symon  (1971)),  spherical  angles, 
and  quaternions  (Hamilton  (1969)).  For  example,  if  the  Euler  angles  0  =  ( ip,0,(f> ) 
arc  employed,  then  they  determine  a  3  by  3  rotation  matrix  which  functions  as 
£(©)  in  the  rotation  group.  It  is  convenient  to  identify  the  rotation  operator  with 
its  parameterization,  that  is,  to  express  X  as 

X  —  (x.  y,  z,  ip,  0,  <f>). 

Using  configuration  space,  reasoning  about  the  motion  of  a  complicated  three- 
dimensional  body  amongst  obstacles  may  be  transformed  into  reasoning  about 
a  point  in  a  six  dimensional  configuration  space.  The  transformation  described 
by  Lozano-Pcrez  (1983)  entails  “shrinking”  the  moving  object  to  a  point,  and 
correspondingly  “growing”  the  obstacles.  In  principle,  the  point  may  then  be 
navigated  around  the  grown  obstacles  by  means  of  the  point  navigation  operators 
(above). 

In  this  thesis,  the  point  navigation  operators  will  be  generalized  to  the 
six-dimensional  coniigu ration  space  of  the  classical  Movers’  problem. 

In  order  to  present  our  algorithm  for  planning  in  C-Space,  it  is  necessary  to 
review  the  basics.  We  present  an  introduction  to  representations  in  configuration 
space  at  two  levels:  first,  we  present  an  intuitive  discussion.  Next,  we  present  a 
more  detailed,  slightly  more  mathematically-oriented  exposition.  For  the  sake  of 
readability,  there  is  some  redundancy  in  the  sections.  Those  who  arc  encountering 
configuration  space  for  the  first  time  may  wish  to  postpone  reading  the  latter 
section  for  now. 
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Figure  1.29.  These  figures  show  an  obstacle  polygon  D  and  a  moving  polygon  A.  A  must  be 
moved  around  0  to  the  goal  configuration.  _  ' _ _ _ _____ 

1.3.1.  Representations  in  Configuration  Space:  An  Intuitive  Treatment 


Figure  1.29  is  an  example  of  the  classical  Movers’  problem  in  two  dimensions, 

_ .  :*1 _ 1 - A  ir.  #.  mnu'in/f  nKiort  ■w/Kirh  TTmsf,  bo.  moved  from  the  start 

WlLliU  11 1/  lUbatlUU9<  -t  *  ®  **iw  Iii»b  ^  ~  J - -  ’  ■  —  -  ’ 

configuration  to  the  goal  configuration,  around  an  obstacle  polygon  B.  The  start 
and  goal  configurations  may  be  expressed  as  two  dimensional  vectors  of  the  form 
(x,  y)  which  represent  the  displacement  of  a  vertex  vx  cn  A  from  a  fixed  coordinate 
frame.  The  displacement  is  a  rigid  translath  of  the  polygon  A.  The  C7-5pace  of 
this  Movers’  problem  is  the  space  of  two  imensional  translations,  which  is  the 
same  as  the  Cartesian  plane.  Tozano-Perez  (1983)  demonstrated  a  transformation 
which  shrinks  A  to  the  vertex  vlt  while  inversely  growing  B.  The  grown  obstacle  for 
B  is  a  C- Space  obstacle  called  CO[D ),  and  is  shown  in  figure  1.31.  (We  will  discuss 
the  details  of  this  transformation  later).  The  problem  of  moving  the  polygon  A 
from  the  start  to  the  goal  is  transformed  into  the  problem  of  navigating  the  point 
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Figure-  1.30.  The  c.ig<rs  end  vertices  of  .4  tu;d  B. 


i>l  around  the  C-Space.  obstacle  shown  in  figure  1.31. 

Both  A  and  }3  are  convex;  non- convex,  objects  arc  represented  by  overlapping 
union*  of  convex  polyhedra.  The  C-Spa^c  obstacle  CO(B)  is  constructed  by 
considering  all  feasible  interactions  of  the  edges  and  vertices  of  A  and  B .  Each  such 
interaction  generates  a  constraint  which  is  m.'.nifest  as  an  edge  of  CO(B ).  We  any 
that  an  interaction  between  a  vertex  of  A  and  an  edge  of  13,  or  between  an  edge  of 
A  and  a  vertex  cf  B,  is  feasible  if  there  is  some  pure  translation  which  can  bring 
the  vertex  and  edge  in  contact  without  causing  A  and  '?  to  overlap.  For  example, 
the  set  of  al*  possible  interactions  of  A  and  B  is  the  union  of  the  two  cartesian 
products 

{«i,e2,C3  }  X  {vJ.Vg.faX  } 
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Figure  1.31.  CO(D),  the  grown  obstacle  for  B  in  C-Spaee.  The  vertex  uj  must  be  navigated 
abound  CO(B). 


v3  }  X  {«!,« 2»c3>c4  }‘ 

However,  at  the  depicted  orientation  of  A,  only  these  interactions  are  feasible: 

{  («l .  e{),  («i ,  e'2),  (ei,v2),  (v2,  e3),  {ei,v\),  (v3)  e'J,  (e3,  v{)  }. 

It  is  easy  to  visualize  the  translation  that  'will  bring  any  of  these  pairs  into  contact. 

Furthermore,  note  that  (for  example),  no  translation  can  bring  v3  in  contact  with 
* 

<2- 

Now  imagine  that  A  is  allowed  to  rotate  about  rj.  At  different  orientations, 
there  will  be  different  sets  of  feasible  contacts.  We  say  that  the  constraints  associated 
with  feasible  contacts  arc  applicable  constraints.  It  should  be  clear  that  at  any  given 
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orientation,  only  certain  constraints  will  be  applicable.  The  orientations  for  which  a 
given  constraint  is  applicable  form  its  applicability  region.  The  applicability  regions 
for  each  constraint  in  this  problem  are  angular  sectors  of  the  form  [<?/  <  6  < 

Tiiis  simply  means  that  there  exists  a  range  of  angles  in  which  a  particular  contact 
is  feasible.  This  range  of  angles  may  be  geometrically  visualized  as  a  sector  of  a 
circle. 


When  A  is  allowed  to  rotate,  the  geometry  of  CO(I3 )  changes  as  6  varies,  and 
as  the  set  of  applicable  constraints  changes.  As  the  edges  and  vertices  of  A  rotate 
about  i>i,  the  constraints  they  generate  sweep  out  ruled  surfaces  which  bound 
CO(I3)  in  a  three  dimensional  C- Space.  (The  C -Space  is  three  dimensional,  since 
A  now  has  three  degrees  of  freedom:  x,  y,  and  0).  At  any  fixed  orientation  Qq,  an 
x-y  slice  of  CO(D )  is  a  polygon,  called  slice(CO[D),  Oq).  Figure  1.31  shows  such  a 
slice  at  the  depicted  orientation  of  A.  With  each  edge  of  slice(CO(H),  0)  there  is  an 
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associated  half- plane  containing  siice(CO(B),  0),  whose  boundary  contains  the  edge 
(see  figure  1.32).  The  intersection  of  these  half-planes  is  exactly  alicc{CO{B),9). 
As  0  changes,  different  half-planes  are  used  to  construct  slicc(CO(B),0).  By  (l) 
deriving  the  line  equation  of  the  boundary  of  these  half-planes  in  terms  of  the 
orientation  6,  and  (2)  by  determining  the  applicability  region  for  each  half- plane 
constraint,  we  can  characterize  slices  of  the  C-Space  obstacle  slicc{CO(n),  0)  as  0 
changes.  Thus  we  can  characterize  the  three  dimensional  C- Space  obstacle  CO{B). 
This  representation  may  be  used  to  develop  planning  algorithms  for  the  Movers’ 
problem  with  two  translational  and  one  rotational  degrees  of  freedom  (sec  Brooks 
and  Lozano-Perez  (1983)). 

In  this  thesis  we  develop  such  a  representation  for  the  six  degree  of  freedom 
Movers’  problem.  There  are  several  problems  which  must  be  solved.  Because 
the  structure  of  the  rotation  group  is  more  complicated  in  three  dimensions, 
the  applicability  regions  for  constraints  in  a  six  degree  of  freedom  C-Space  are 
geometrically  much  more  complicated.  While  in  two  dimension0  the  applicability 
regions  may  be  visualized  as  sectors  on  a  circle,  in  three  dimensions  they  are 
complicated  three  dimensional  manifolds  on  the  projective  3-sphere.  It  is  important 
to  characterize  these  regions,  since  they  specify  where  a  constraint  is  applicable. 
We  will  discuss  some  of  the  other  problems  presently. 

Generalizing  the  Point  Navigation  Operators  Requires  Solving 
Representational  Questions 

In  order  to  generalize  the  point  navigation  operators  to  the  C-Space.  of  the 
classical  Movers’  problem,  we  must  be  able  to  characterize  the  surfaces  of  C-Space 
obstacles,  and  the  intersections  of  these  surfaces.  The  first  two  operators,  then,  must 
slide  along  the  C-surfaces  and  their  intersections.  In  the  next  section,  we  discuss  some 
of  the  representational  issues  involved  in  developing  such  operators.  For  example, 
when  rotations  arc  allowed,  the  C-surfaces  arc  curved.  In  the  six-dimensional  space 
of  the  classical  Movers’  problem,  each  C-surface  is  a  five-dimensional  submanifold  of 
C-Space,  and  the  intersection  of  two  such  surfaces  i:,  i  four-dimensional  manifold. 
Thus  it  is  possible  to  slide  along  such  an  intersection  with  four  degrees  of  freedom. 
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1.3.2.  Representations  in  Configuration  Space:  A  More  Formal  Treatment 

In  this  section,  we  present  a  somewhat  more  abstract  formulation  of 
representational  issues  in  C-Space.  Some  readers  may  wish  to  postpone  reading  this 
section  until  later.  Wc  will  proceed  as  follows:  Gist,  we  will  outline  an  important 
representational  question  which  must  be  solved  in  this  thesis.  Next,  we  discuss 
liow  to  represent  volumes  (such  as  C-Space  obstacles)  in  C-Space.  In  the  course 
of  this  discussion,  several  terms  will  be  defined  in  context  by  means  of  intuitive 
descriptions.  At  the  end  of  this  section,  under  the  heading  Working  Definitions,  wc 
will  summarize  and  formalize  the  definitions  to  the  extent  that  will  be  required  in 
chapters  1  and  2. 

The  Domain  Question 

Until  now,  geometric  planning  problems  with  more  than  three  degrees  of 
freedom  have  proved  resistant  to  solution.1  In  this  thesis  we  provide  such  an 
algorithm  for  find-path  with  six  degrees  of  freedom  (the  classical  Movers’  problem). 
The  resistance  cl"  these  problems  has  largely  been  due  to  unresolved  mathematical 
issues  and  questions  relating  to  the  structure  of  configuration  space  and  to  the  nature 
of  C-Space  constraints,  (although  for  fine-motion  and  planning  with  uncertainty 
there  arc  of  course  additional  issues). 

One  fundamental  theoretical  problem  for  high-dimensional  configuration  spaces 
may  bo  stated  as  follows:  in  a  configuration  space  C  with  rotations,  each  C-Space 
obstacle  may  be  represented  by  the  intersection  of  a  finite  number  of  half-spaces. 
Each  half-space,  in  turn,  is  defined  by  a  real-valued  C-function  on  C-Space.  For 
example,  the  half-space  might  be  the  set  of  configurations  where  the  C-function 
is  negative.  However,  each  C-function  is  a  partial  function  on  C,  whose  domain 
is  a  complicated  region  in  C -Space.  This  greatly  complicates  the  representation 
for  C-Space  obstacles  and  C-surfaccs  (see  figure  1.3G).  Moreover,  until  now'  the 
domains  of  the  C-functions  were  unknown  for  all  but  the  onc-diincnsional  rotation 
group.  One  of  our  first  tasks  will  be  to  derive  the  domains  of  all  C-functions  for 
the  classical  Movers’  problem  with  six  degrees  of  freedom. 

'However,  previous  work  lias  provided  an  existence  proof  of  a  polynomial  time  algorithm  for 
certain  spatial  planum)'  problems.  In  addition,  there  are  approximate  algorithms  for  some  o[  these 
problems.  See  our  review  of  previous  work. 
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There  are  several  related  problems,  for  which  wc  also  present  solutions.  This 
allows  us  to  construct  a  complete  geometric  representation  for  the  configuration  space 
of  the  classical  Movers’  problem  with  six  degrees  of  freedom.  This  representation 
impacts  all  the  geometric  planning  problems  we  have  discussed,  and  extends 
naturally  to  Cartesian  Manipulators. 

Representing  Volumes  in  Configuration  Space 

The  dimensionality  of  configuration  space  is  the  number  of  degrees  of  freedom 
in  the  parameter  space,  i.c.,  the  number  of  degrees  of  freedom  available  to 
the  moving  objcct(s).  Thus  the  classical  Mover’s  problem  in  the  plane  has  two 
translational  and  one  rotational  degrees  of  freedom,  while  in  three  dimensions  it 
has  three  translational  and  three  rotational  degrees  of  freedom.  The  configuration 
spaces  for  these  problems  arc  three  and  six  dimensional  manifolds,  respectively. 
As  the  number  of  degrees  of  freedom  increases,  a  geometric  planning  problem 
becomes  harder.  There  arc  several  reasons  for  this.  First  of  all,  when  rotations  arc 
allowed,  configuration  space  ceases  to  be  Euclidean,  and  the  C-surfaccs  become 
curved.  Furthermore,  the  non-commutativity  and  multiple-connectedness  of  the 
three-dimensional  rotation  group  are  classical  difficult  issues  in  mathematics.  In 
addition,  it  can  be  shown  that  the  computational  complexity  of  spatial  planning 
grows  exponentially  with  the  dimensionality  of  the  C-Space. 

A  fundamental  issue  for  geometric  planning  algorithms  is:  how  should  C-Space 
obstacles  and  surfaces  be  represented  and  computed? 

A  volume  in  a  configuration  space  C  may  be  represented  by  the  intersection  of 
a  finite  number  of  half-spaces  (sec  figure  1.33).  Each  half-space  may  be  defined  via 
some  smooth,  real-valued  function  function  on  C, 


fi-x  -  ut. 

For  example,  (fig.  1.34)  suppose  /,(x,y)  --=■■  ax  -f  by  -t-  c,  for  some  constants  a,  b, 
and  c.  The  kernel  of  /  is  the  line  where  f{x,y)  =  0.  The  halfspacc  h~  is  the 
portion  of  the  plane  where  f(x,y)  is  negative.  C-ftmctions  such  as  /,  arise  in  the 
two  dimensional  Movers’  problem,  at  a  fixed  orientation. 


Figure  1.33-  The  region  CO  is  the  intersection  of  the  half-spaces  h,  ,  h~ ,  hk  ,  and  h]  . 

In  general,  the  (closed)  half-space  h ^  is  the  set  of  all  points  in  C  where  fi  is 
negative-valued  or  zero: 


h,  =  {v  fc  C  j  fi(v)  <  0}. 

The  common  intersection  of  a  number  of  such  half-spaces  can  yield  a  volume  in 
C.  Lozano-Perez  (1983)  showed  how  C-Space  obstacles  can  be  modeled  in  this 
manner,  and  gave  the  form  of  the  functions  /,.  Note  that  each  C-surface  lies 
within  the  kernel  of  some  constraint  /,.  Fine-motion  strategies  and  algorithms  for 
planning  with  uncertainty  need  to  compute  the  normals  and  tangent  spaces  to 
these  C-surfaces.  The  normal  can  usually  be  derived  from  the  gradient  V/,  (this 
requires  placing  an  appropriate  Riemannian  metric  on  the  tangent  space).  When 
a  real-valued  function  /„  on  configuration  space  is  used  to  describe  constraints  in 
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that  C-Space,  (i.c.,  C-Space  obstacles),  we  call  it  a  C- function.  The  form  and 
interpretation  of  C-functions  are  presented  later. 


Figure  1.35.  Illustration  of  the  classical  Mover's  problem  in  three  dimensions.  H  is  an  obstacle, 
and  A  is  an  object  which  must  l>e  moved  around  i).  /!»,«,  shows  A  in  the  start  configuration, 
and  shows  A  in  the  desired  goal  configuration.  C  is  the  polyhedron  which  is  the  C-Space 

obstacle  from  H  for  A  at  orientation  0|.  At  orientation  the  ('-Sparc  obstacle  from  li  is  a 
different  polyhedron,  which  we  show  as  C . 
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Example-.  (See  figure  1.35).  Consider  the  Movers’  problem  for  a  three  dimensional 
polyhedron  A  which  can  translate  but  not  rotate  amidst  polyhedral  obstacles.  The 
configuration  space  for  A  is  a  three-dimensional  vector  space  of  translations,  which 
it  is  convenient  to  identify  with  HJ'1.  Each  constraint  /,  will  be  linear  on  'Ji'1,  and 
the  kernel  of  /,  is  a  plane.  Each  such  plane  bounds  a  C-Space  obstacle  (such  as  C 
or  C'  in  figure  1.35).  The  C-Space  obstacles  are  possibly  overlapping  polyhedra  in 
Hi'1.  The  find-path  problem  in  the  transformed  space  is  that  of  navigating  a  point 
past  the  union  of  these  C-Space  polyhedra  (see  Ggure  1.39). 

In  this  formulation  there  is  a  fundamental  underlying  assumption:  /,•  is  a  total 
function  on  C,  that  is,  the  domain  of  /,  is  all  of  C.  In  the  example  this  is  not  a 
problem,  since  the  domain  of  each  linear  C  function  is  the  entire  space.  A  function 
whose  domain  is  a  subset  of  C  is  called  a  partial  function  on  C.  When  rotations  are 
allowed,  C-functions  become  partial  functions. 
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Figure  1.36.  The  function*  /,,  fk,  and  Ik  are  used  to  describe  the  liaif-spaccs  h,  ,  h}  ,  and 
h If  all  functions  arc  total  functions  on  the  plane,  then  the  intersection  of  the  corresponding 
half-spaces  will  be  the  the  rcclangloid  region  R.  However,  suppose  that  /,  is  a  partial  function, 
whose  domain  is  restricted  to  the  half  space  where  y  is  positive.  We  sav  that  /,  is  not  applicable 
below  the  line  y  —  Q.  Furthermore,  we  assume  that  points  outside  the  domain  of  /,  arc  within 
h~.  In  this  case,  the  intersection  ht  f|  K  D  hk  '»  triangloid  region  T. 
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Figure  1.37.  The  two-dimensional  classical  Movers’  problem:  An  obstacle  polygon  B  and  a  moving 
polygon  A.  A  is  shown  at  a  particular  orientation,  0j. 


Why  do  C-Space  constraints  become  partial  functions  when  rotations  are 
introduced?  Consider  the  classical  find-path  problem  in  two  dimensions,  for  a 
moving  polygon  A  which  can  translate  and  rotate  in  the  plane  (see  figure  1.37).  A 
configuration  of  A  may  be  represented  by  three  parameters,  (x,  y,  0).  The  surfaces  of 
the  C-Space  obstacle  for  B  arise  from  each  of  the  feasible  contacts  (or  interactions) 
between  the  edges  and  vertices  of  A  and  B.  Thus  the  constraint  functions  (which 
we  have  been  calling  /*•)  are  defined  by  considering  pairwise  interactions  of  edges 
and  vertices  of  A  with  vertices  and  edges  of  B.  Every  such  pair  such  as  e0  and 
will  generate  a  smooth,  real-valued  C-function  fea)Vb  on  configuration  space.2  Each 
constraint  is  designed  such  that  their  conjunction  enforces  a  disjointness  criterion 
for  A  and  B.  However,  not  all  interactions  are  possible  at  any  given  orientation. 
For  example,  at  the  depicted  orientation  0\  of  A,  edge  can  interact  with  vertex 
v,  but  edge  r.\  cannot:  at  orientation  6\,  no  translation  can  bring  e\  in  contact 
aFor  the  form  of  the  O-functions,  see  chapters  3  and  4, 


with  v  while  maintaining  the  disjointness  of  the  interiors  of  A  and  D.  We  say  the 
associated  C-function  fr.x,v  *s  n°l  applicable  at  orientation  0\.  In  other  words,  no 
configuration 


(i,j /,<?!)€  SR2  X  {Oi  } 

is  in  the  domain  of  /eii„.  Each  constraint  fCaiVl>  is  applicable  only  at  certain 
orientations,  and  hence  each  can  be  considered  a  partial  function  on  the  C-Space .3 

In  three  dimensions  (see  figures  1.35  and  1.39),  the  surfaces  of  the  C-Space 
obstacle  for  13  arise  from  each  of  the  feasible  contacts  between  the  vertices,  edges, 
and  faces  of  A  and  D.  By  analogy  with  figure  1.37,  it  is  clear  that  not  all  of  these 
interactions  are  feasible  at  any  given  orientation.  Thus  the  C-functions  describing 
C-surfaces  for  spatial  planning  with  six  degrees  of  freedom  must  also  be  partial 
functions. 

Working  Definitions:  Review  and  Summary 

Wc  now  summarize  and  formalize  the  key  definitions  and  concepts  required  in 
this  chapter  and  the  next: 

Configuration  Space :  (Formal  definition)  Configuration  space  is  the  product 
space  of  the  space  of  translations  and  the  space  of  rotations  for  an  object.  In  three 
dimensions,  the  space  of  translations  is  Euclidean  3-space  and  the  space  of 
rotations  is  the  3-dimcnsional  rotation  group  or  Special  Orthogonal  Group.  50(3). 
50(3)  is  isomorphic  to  the  intersection  of  the  Special  Linear  Group  (the  set  of 
all  real  3X3  matrices  with  determinant  1)  and  the  Orthogonal  Group  (which 
may  be  thought  of  as  the  set  of  matrices  with  orthononnal  rows  and  columns). 
The  orthonormality  of  rows  and  of  columns  are  equivalent  conditions.  50(3)  is 
isomorphic  to  P^ ,  the  3-sphcrc  5^  with  opposite  points  identified.  [P^  is  also  known 
as  the  projective  S-sphcre).  53  is  isomorphic  the  group  of  unit  quaternions.  For  the 
classical  Movers’  problem  wc  will  employ  configuration  space,  X  50(3).  Wc  will 
denote  the  classical  Movers’  Problem  with  three  translational  and  three  rotational 

8 See  Brooks  ;unl  Lozano- 1  Vrcz  ( 1 983)  for  a  discussion  of  tin-  domains  of  (’-functions  for  tlic  two 
dimensional  lind-jtal.li  problem  with  rotations. 
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degrees  of  freedom  as  6 DOF.  In  practice,  we  will  represent  rotations  as  members 
of  a  three-parameter  family  (for  example,  Euler  Angles),  but  we  must  keep  in  mind 
that  they  parameterize  an  isometry  and  that  9?'*  X  50(3)  is  not  a  vector  space. 
If  the  Euler  angles  (i p,0,<j>)  are  employed  to  represent  the  orientation  of  a  rigid 
polyhedral  body,  a  typical  configuration  X  in  9i !  X  50(3)  has  the  form 

X  =  (x,y,z,tp.O,<j>). 


We  will  sometimes  adopt  the  notation 

X  =  (r,  0), 

where  r  denotes  a  three-dimensional  translation  vector,  and  0  some  three 
dimensional  rotation.  This  second  notation  is  independent  of  the  particular 
representation  chosen  for  rotations;  the  first  isn’t.  If  Euler  angles  are  employed,  we 
may  think  of  0  as  the  “vector"  of  Euler  angles,  (t/>,  0,  $). 

C -Space  Obstacle:  (Informal  definition)  Configuration  space  obstacles  are 
(possibly  overlapping)  six  dimensional  manifolds  (with  boundary)  which  correspond 
to  sets  of  configurations  that  would  cause  collisions  of  the  moving  object  with  real 
space  obstacles. 

Free  space:  The  free  space  is  that  subset  of  C-Space  which  lies  within  no 
C-Spacc  obstacle.  The  free  space  will  be  denoted  F. 

Applicability  Set:  (Informal  definition)  liefer  to  figures  l. 29-32  and  1.33-38,  and 
recall  that  C-Space  obstacles  arc  represented  by  the  intersection  of  a  finite  number 
of  half-spaces.  (To  be  formal  we  should  call  them  half  hypcrspaces).  The  boundary 
of  each  half-space  is  a.  C-surlace,  and  contains  a  boundary  patch  of  the  C-Space 
obstacle.  Each  C-surface  5  may  be  expressed  as  the  kernel  of  a  real-valued  function 
/  on  C-Spacc.  The  C-function  f  is  negative  on  the  obstacle  side  of  the  half-space 
C-Space  obstacle,  and  positive  on  the  other  half.  In  the  literature  C-functions  have 
been  called  constrairils,  since  they  express  constraints  on  the  possible  motions  for 
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an  object.  A  surface  parallel  to  S  is  called  a  level  C-surface,  and  represents  the  set 
of  configurations  where  /  has  a  certain  fixed  value.  This  value  is  termed  the  level 
of  the  level  C-surface.  The  boundary  of  the  C-Space  obstacle  is  a  special  case  of 
level  C-  surface,  where  the  level  is  zero.  We  have  seen  that  at  any  given  orientation, 
only  certain  C-functions  (and  their  associated  C-surfaces)  are  applicable.  This  is 
because  only  certain  contacts  are  feasible  between  the  faces,  edges,  and  vertices 
which  generate  the  C-functions.  Wc  call  this  set  of  C-functions  the  applicability  set. 
For  example,  in  1.29-32,  at  the  depicted  orientation  the  applicability  set  is 

{ («i. «{),  (^1  >  C2),  («i,  v.^),  (ua,  63),  (e2,  v{),  (t;3,  e[),  (e3)  v{)  }. 

(Actually,  the  applicability  set  is  the  set  of  C-functions  generated  by  these 
vertex/edge  and  edge/vertex  pairs,  but  since  there  is  a  one-to-one  correspondence 
between  the  generator  pairs  and  the  C-functions,  we  can  write  it  this  way).  In  later 
chapters,  we  will  demonstrate  algorithms  for  computing  the  applicability  set,  and 
for  decomposing  rotat:o»i  space  into  regions  where  the  applicability  set  is  invariant. 

Redundant  and  Non-redundant  Constraints:  (Informal  definition).  IF  a  config¬ 
uration  X  is  in  free  space,  the  set  of  constraints  which  is  (locally)  relevant  to  motion 
planning  from  X  is  a  subset  of  the  applicable,  positive- valued  C-functions  at  X. 
However,  the  value  of  a  C- function  docs  more  than  merely  indicate  which  side  of  a 
C-surface  X  is  on.  A  C-S'unction’s  value  represents  the  translational  distance  to  that 
surface.  Thus,  C-functions  provide  a  collection  of  pseudo-metrics  on  C-Spucc.  Using 
these  metrics,  it  is  possible  to  order  C-surfaces  by  their  closeness  to  a  configuration 
A^  (simply  sort  the  C-functions  on  their  value  at  X).  We  say  that  a  C-surface  is 
redundant  if  it  is  subsumed  by  a  nearer,  intervening  constraint.  In  figure  1  38,  for 
example,  f  and  g  arc  non- redundant  constraints  at  X,  but  h  is  redundant  since  it 
is  subsumed  by  /.  It  is  useful  to  determine  the  set  of  non-redundant  constraints  at 
X  since  this  is  the  ’inallest  set  of  constraints  that  are  locally  relevant  to  motion 
planning.  We  provide  a  formal  definition  «r  redundancy  in  chapter  3. 

Robot,  Moving  Object,  and  Piano:  All  of  these  terms  have  been  employed  in 
the  literature  to  refer  to  the  moving  object  for  which  a  collision- free  path  must 
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The  Research  Agenda  of  this  Thesis 
A  Brief  Outline 

I.  Computational  Theory 

Paths  can  be  found  in  C-Space  by  the  closure  of  three  operators: 

(i)  slides  along  1-  to  4-dimcnsional  intersections  of  level  C-surfaces; 

(ii)  slides  along  5-diniensional  level  C-surfaces; 

(iii)  jumps  between  6-dimensional  obstacles. 


EL  Representation  and  Algorithm 
Search  Algorithm  Employing  the  Three  Operators 

tr 

Solve  the  Intersection  Problems. 

Develop  a  represention  for  the  intersection  manifolds. 

tr 


1.3.3.  Generalizing  the  Point  Navigation  Operators 

Consider  a  thiec-dimcnsional  configuration  space  containing  smooth,  curved 
C-Space  obstacles.  Observe  that  the  point  navigation  operators  will  work  even  if 
the  surfaces  are  curved  and  complicated,  as  long  as  we  can  find  their  intersections, 
for  the  two-dimensional  Mover’s  problem  (for  a  polygon  allowed  to  rotate  and 
translate  in  the  plane),  we  employ  a  configuration  space  ?R2  X  S1  •  9i2  is  the  space  of 
two  dimensional  translations,  and  Sl  is  the  unit  circle,  on  which  one-dimensional 
rotations  may  be  represented.  C-functions  are  of  the  form  /,•  :  3R2  X  S1  — »  3i  and 
are  valid  within  some  sector  Ai  of  •S'1.  A  C-surface  is  the  set  of  configurations 
where  /,•  is  zero.  Although  these  C-functions  arc  complicated  expressions  containing 
trigonometric  terms  of  the  form  xcosO  and  y  sin  0,  it  is  possible  to  solve  two 
such  C-surfaces  simultaneously  to  obtain  an  intersection  curve  in  9?2  X  •S1  which 
is  parametric  in  0  (these  intersection  manifolds  are  derived  in  chapter  4).  The 
analogy  to  navigating  a  point  through  a  polyhedral  environment  should  now  become, 
clear:  the  faces  of  the  polyhedra  correspond  to  C-surfaces  in  SR2  X  Sl  and  the 
edge-graphs  to  the  graph  of  C-surfacc  intersections.  By  searching  the  graph  of 
C-surfacc  intersections  we  can  find  a  path  in  configuration  space,  if  one  exists. 

Planning  in  a  Six  Dimensional  C-Space 

Our  planner  for  a  six  dimensional  C-Space  is  based  on  the  idea  of  moving  along 
the  intersections  of  level  C-surfaces  in  free  space,  parallel  to  the  boundaries  of  C- 
Space  obstacles.  In  the  example  above,  the  coincidence  between  the  dimensionality 
of  configuration  space  and  Euclidean  space  v/as  serendipitous:  edges  on  polyhedra 
corresponded  to  curves  in  9J2  X  5*,  and  faces  to  2-dimcnsional  surfaces.  However,  in 
a  six  dimensional  C-Space,  the  C-surfaccy  are  5-dimensional  and  their  intersections 
are  4-dimcnsiona!  sub-manifolds.  Intuitively  this  means  that  the  set  of  possible 
motions  while  complying  with  two  constraints  is  a  four-parameter  family. 

Our  idea  is  as  follows:  Suppose  wu  could  slide  along  C-surfaces  (see  figure  1.39). 
In  addition,  suppose  we  could  intersect  C-surfaces  to  construct  a  lower  dimensional 
manifold  in  C-Space  which  contained  paths  along  (or  around)  the  boundary  of 
C-Space  obstacles.  Dy  sliding  along  C-surfaces,  and  by  slicing  along  the  intersection 
of  C-surfaces,  wc  should  be  able  to  devise  an  algorithm  which  can  circumnavigate 
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Figure  1-39.  We  can  represent  the  configuration  of  &  polyhedron  A  by  a  pair,  (T,  0),  where  7  is 
a  translation  of  A  and  ©  is  a  rotation  of  A.  The  problem  of  moving  A  from  configuration  (r,,©i) 
to  (rff,0a)  is  transformed  to  the  problem  of  navigating  a  conjiguration  point,  r,  past  C,  which 
is  the  C-Space  obstacle  due  to  V.  S,  and  S2  arc  C-surfaces  bounding  G.  The  configurations  c< 
lie  oft  the  bounds  y  of  C,  while  d*  is  in  frec-space.  Two  trajectories  around  U  are  shown.  Note 
that  the  path  segments  (c6, (r„, ©2))  and  (dl;(rs,©2))  must  also  include  a  rotation.  (The  actual 
reference  point  is  at  the  centroid  of  A,  but  for  the  purposes  of  exposition,  we  have  placed  it  at  a 
vertex  as  shown). 

C-Space  obstacles.  (Of  course,  we  also  need  a  way  to  plan  motions  which  “jump” 
from  one  obstacle  to  another). 

Example:  Figure  1.39  shows  how  such  a  planning  algorithm  might  work.  The 
planner  moves  through  free-space  from  (ra,©i)  until  it  strikes  a  C-surface  S i  at 
cj.  From  Cj  a  path  is  found  towards  C2  sliding  along  the  C-surfacc  Si-  We  say  the 
planner  slides  along  Si  to  C2.  Configuration  c%  lies  on  an  intersection  manifold  of 
the  C-surfaces  Si  and  S2.  The  path  segment  (c2,c3)  slides  along  this  intersection 
manifold,  which  lies  on  the  boundary  of  C.  A  path  (c2»c3»c-J>c5>cg)  is  planned  along 
the  graph  of  intersection  manifolds  on  the  boundary  of  C.  From  C&  we  leave  the 
boundary  of  C,  and  translate  and  rotate  through  frec-spacc  to  (re,©2)-  This  path 
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is  an  idealized  example  of  planning  along  C-surfaces  and  intersection  manifolds  of 
C-surfaces.  The  implemented  planner  finds  a  path  similar  to  ((riS,  ©i),  d\ ,  (rgi  ©2)) 
(see  chapter  2),  The  path  segment  ((rv,  O),  dj)  is  parallel  to  the  C-surface  S i,  and 
we  say  that  it  slides  along  a  level  C-surface  for  S\.  The  path  segment  (d|,(r?,0)) 
is  along  a  level  C-surface  for  S^.  These  level  C-surfaces  intersect  along  a  manifold 
in  free-space  containing  di  (imagine  extending  the  faces  S[  and  S2  beyond  the 
boundary  of  C  until  they  intersect). 

Wc  will  derive  the  necessary  mathematical  theory  and  tools  relating  to  C- 
surfaccs  and  their  intersection  manifolds,  and  present  algorithms  for  moving  and 
planning  paths  in  C- Space.  Some  of  the  issues  we  will  address  include: 

(i)  What  is  an  appropriate  representation  for  constraints  in  a  six  dimensional 
C-Space ? 

(ii)  How  do  we  plan  motions  using  constraints  whose  domains  change  with  the 
motions? 

(iii)  How  can  trajectories  in  C-Space  be  intersected  with  C  surfaces  whose  domains 
change  along  the  trajectory? 

(iv)  flow  can  intersection  manifolds  be  constructed  in  C-Space ? 

(v)  IIow  are  motions  planned  which  slide  along  C-surfaces  and  intersection  manifolds? 

It  is  useful  to  develop  a  terminology  for  evaluating  algorithms  and  repre¬ 
sentations  for  geometric  planning  problems.  An  algorithm  employing  an  approximate 
representation  does  not  characterize  the  constraints  exactly.  A  complete  algorithm 
(for  a  given  resolution)  is  guaranteed  to  find  a  solution  if  one  exists  (at  that 

T"  /TAf>  nro  1  />  /N  *1  n  f  f*1i  t  »  rv  r*  #irt  »»»  T\  1  rt  ♦  r\  «  1  rr/\  K  rvi  •»>%  n  f  n 
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ment  of  a  complete  representation.  A  brute-force  algorithm  tries  to  find  a  solution 
through  exhaustive  search.  Heuristic  algorithms  fall  into  two  (overlapping)  classes: 
heuristically  complete,  and  heuristically  fast.  See  also  the  review  of  previous  work 
(below). 

The  implemented  algorithm  we  present  for  the  classical  Mover’s  proHem  with 
six  degrees  of  freedom  employs  a  complete  representation  of  the  configuration  space 
constraints,  and  a  complete  search  algorithm  (for  a  given  resolution). 


1.4.  Local  versus  Global 


Local  algorithms  for  the  find-path  problem  examine  local  constraints  in  some 
neighborhood  of  real  space  or  in  C-Space,  and  propose  motions  based  on  the 
geometry  of  the  neighborhood.  Typically,  local  algorithms  are  implemented  as 
searches,  and  the  examination  of  constraints  near  a  search  node  leads  to  the 
selection  and  application  of  some  local  operator  to  move  the  robot  in  space.  For 
example,  in  our  algorithm  for  the  six  degree  of  freedom  Movers’  problem,  the  local 
constraints  correspond  to  the  geometric  structure  of  C-surfaces  in  a  neighborhood, 
and  local  operators  consist  of  motions  along  C-surfaces.1  In  general,  a  local  planning 
algorithm  will  be  complete  if  (l)  the  closure  of  the  local  operators  is  complete  for  the 
arcwise-connected  components  of  C-Spa.ce,  and  (2)  each  local  operator  attempted 
ensures  that  a  collision-free  path  exists  between  configurations  in  the  search. 

However,  our  observation  has  been  that  in  general,  even  complete  local 
algorithms  can  get  lost  examining  irrelevant  local  constraints.  In  particular,  without 
adequate  knowledge  of  the  connectivity  of  a  workspace  and  the  classes  of  paths 
it  contains,  such  methods  may  choose  impossible  or  ill-advised  candidate  paths: 
hence  they  may  take  a  long  time  to  converge. 

A  global  find-path  algorithm  attempts  to  construct  a  model  of  the  connectivity 
of  the  workspace.  We  believe  that  the  connectivity  cf  configuration  space  can  be 
inferred  from  the  connectivity  of  real  space.  Good  hypotheses  about  the  channels, 
or  classes  of  paths  through  frec-space  can  serve  as  guidance  for  a  more  detailed 
method.  While  there  exist  several  proposals  for  global  approaches  to  the  Movers’ 
problem,  in  Donald  (1983a)  we  attempt  to  formalize  criteria  for  the  design  of  such 
algorithms.  A  global  planner  based  on  these  criteria  was  implemented,  and  coupled 
with  a.  complete  local  algorithm  to  form  an  integrated  planning  system. 

Channels  arc  an  encoding  of  free-space  corresponding  to  the  classes  or  paths 
within  an  environment.  An  implementation  exploiting  this  global  model  of  the 
connectivity  of  frec-spacc  has  been  able  to  solve  two  dimensional  find-path  problems 
in  several  minutes  which  formerly  took  many  hours.  The  algorithm  is  essentially 

'Thin  example  is  illiist rntive  and  typical  of  the  local  constraints  and  operators.  The  implemented 
planner  is  more  complicated,  as  we  shall  see. 
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a  problem-solving  strategy  using  a  homcomorphic  reduction  of  the  search  space. 
Sec  Donald  (1983a)  for  a  description  of  the  channel  algorithm.  In  appendix  III, 
we  discuss  the  design  and  integration  of  local  and  global  planning  algorithms  in 
SR3  X  50(3). 


1.5.  Review  of  Previous  Work 


1.5.1.  Introduction 


In  this  section  we  review  previous  work  on  geometric  planning  problems. 
We  also  give  a  formal  characterization  of  completeness  for  the  spatial  planning 
problems.  A  survey  of  robotics  issues  in  robot  motion  planning  can  be  found 
in  Brady,  et  <xl.  (1980),  For  related  work  on  the  mover’s  problem,  see  Brooks 
(1983a),  Lozano-Percz  (1981,  1983),  Lozano-Pcrez  and  Wesley  (1979),  Brooks  and 
Lozano-Percz  (1983),  Schwartz  and  Sharir  (1982a),  Reif  (1979),  Moravec  (1979), 
Udupa  (1977)  and  Ilopcroft  and  Wilfong  (1984).  Wingham  (1977)  and  Popplcstone, 
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Some  issues  in  automated  structural  design  arc  addressed  in  Donald  (1983b).  For 
a  review  of  geometric  modeling  techniques,  see  Baer,  Eastman,  el  al.  (1979)  and 
Rcquicha  (1980). 


1.5.2.  Complexity-Theoretic  Results 


In  seminal  work  on  the  complexity  of  the  Movers’  problem,  Rcif  (1979)  has 


shown  that  the  motion  ulannine  moblem  for  a  robot  with  an  arbitrary  number 


of  degrees  of  freedom  in  the  form  of  arm-like  linkages  is  P-Space-  hard.  Ilopcroft, 
Joseph,  and  Whitesides  (1982)  have  shown  similar  results  for  planar  manipulators 
with  n  linkages.  In  general  it  has  been  found  that  with  n  degrees  of  freedom,  the 
problem  is  P-L Space-hard.  Happily,  Schwartz  and  Sharir  (1982a)  have  demonstrated 
the  existence  of  a  polynomial- time  algorithm  for  the  Movers’  problem  with  Fixed 
degrees  of  freedom,  where  the  size  of  the  problem  is  measured  in  the  number  of 
obstacle  faces  in  the  environment.  The  algorithm  of  Schwartz  and  Sharir  (1982a) 
for  the  classical  Movers’  problem  is  unfortunately  of  time  complexity 
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where  n  is  polynomially  dependent  on  the  number  of  faces  in  the  environment,  and  d 
is  the  number  of  degrees  of  freedom.  For  6  degrees  of  freedom  this  becomes  0(n'1006). 
Ilcncc  it  serves  chiefly  as  an  existence  proof  for  a  polynomial-time  algorithm. 


The  coordinated  motion  problem  has  also  been  given  some  attention:  Schwartz 
and  Sharir  (1982b)  address  the  problem  for  2  and  3  circular  bodies  moving  amidst 
polygonal  obstacles  in  the  plane.  The  coordinated  motion  system  has  degrees  of 
freedom  equal  to  the  sum  of  the  degrees  of  freedom  of  the  moving  bodies.  These 
results  lead  us  to  expect  exponential  behavior  from  all  motion- planning  algorithms 
as  the  number  of  degrees  of  freedom  grows.  For  these  reasons  we  will  confine 
ourselves  to  the  classical  Movers’  problem,  which  has  G  degrees  of  freedom  in 
3-dimcnsional  space. 


Reif  (1979)  also  sketches  a  polynomial-time  algorithm  for  the  classical  Movers’ 
problem,  but  it  appears  incomplete  in  that  it  ignores  constraints  arising  from  the 
interactions  of  faces  of  the  moving  object  with  vortices  of  obstacles,  and  docs  not 
consider  edge-edge  interactions  in  3  dimensions. 


1.5.3.  Work  in  Computational  Geometry  and  Robotics 

The  foundations  of  our  approach  lie  in  Lozano-Perez  (1981 ,  1983),  Lozano-Perez 
and  Wesley  (1979)  and  Schwartz  and  Sharir  (1982a).  The  problem  of  moving  a 
complex  polyhedral  object  among  obstacles  is  transformed  to  the  problem  of  finding 
a  path  for  a  point  in  a  hig’n-uimensiunul  configuration  space. 


Brooks  and  Lozano-Perez  (1983)  have  implemented  a  general  path-finding 
algorithm  for  a  polygonal  object  in  the  plane  with  two  translational  and  one 
rotational  degrees  of  freedom.  Their  planner  uses  hierarchical  subdivision  of  the 
3-dimcnsional  configuration  space  SR2  X  S1.  The  subdivision  algorithm  has  been 
specialized  to  the  particular  geometry  of  the  Movers’  problem  in  9J2  X  S1  and 
while  in  principle  it  is  extensible  to  the  6  degree  of  freedom  problem,  its  space- 
complexity  in  high  dimensions  is  bkcly  to  be  vnutci;  ct>ve  /V  problem  with  the 
hierarchical  subdivision  strategy  is  that  it  i.-is  tioahL  exploiting  rohctcnce  in 
C-Spacc.  Its  spatial  taxonomy  is  restricted  to  filled,  empty,  and  mixed,  in  a 
world  where  almost  everything  is  mixed.  Mixed  cells  are  subdivided  until  an  empty 


region  is  found.  However,  it  is  hard  to  propagate  this  useful  information  to  guide 
the  search  through  nearby,  unrefined  cells  in  the  subdivision.  One  goal  of  the 
algorithms  and  representations  in  this  thesis  has  been  to  exploit  coherence  in  the 
configuration  space.  The  intuitive  appeal  is  that  the  intersections  of  C-surfaces  “go 
somewhere  useful”  (i.c.,  around  the  obstacles).  Wc  will  adopt  an  approacli  which 
exploits  the  coherence  of  C-Space  obstacles  by  moving  along  the  intersections  of 
high-dimensional  C-inanifolds2  parallel  to  the  obstacle  boundaries. 


Lozano-Percz  (L981)  has  described  approximate  solutions  for  Cartesian  manip¬ 
ulators  with  6  degrees  of  freedom  (in  principle)  which  consider  3-dimensional 
slice-projections  of  Configuration  space.  In  practice  these  approximations  are  only 
reasonably  accurate  for  Cartesian  manipulators  with  4  degrees  of  freedom.  In 
principle,  the  C-Space  constraints  on  motion  defined  by  Lozano-Percz  (1983)  can 
be  extended  directly  to  a  6  degree  of  freedom  planner;  indeed,  this  is  our  starting 
point.  However  there  are  many  interesting  and  complex  problems  to  work  out  (see 
Brooks  (J983b)  for  another  discussion  of  these  problems).  In  particular,  there  are 
many  unresolved  mathematical  details  for  the  6  degree  of  freedom  case.  Given 
the  mathematical  model,  there  still  remains  the  issue  of  a  complete  planner  which 
exploits  the  model. 

1.5.4.  Global  Methods 


Global  methods  for  path  planning  attempt  to  construct  a  model  of  the 
connectivity  of  freo-space  which  can  be  related  to  the  Voronoi  diagram  (see 
Drysdaie  (1983)).  In  particular,  Brooks  (i983a)  has  implemented  a  2-aimonsionai 


path-planner  which  models  the  frcc-space  as  an  overlapping  union  of  Generalized 
Cones  (Binford  (1971)).  Each  cone  provides  orientation  constraints  on  motion  within 
the  cone,  and  these  constraints  arc  intersected  to  find  a  translational  path  along  the 
cone  axes  (called  spines )  interspersed  with  rotations  at  the  spine  intersections.  This 
work  was  extended  to  a  six-link  manipulator  for  moving  payloads  with  4  degrees 
of  freedom  (Brooks  (1983b)).  The  extended  algorithm  uses  the  same  cone  model, 
but  sweeps  each  cone  vertically  to  build  prisms  at  horizontal  slices  through  the 
workspace.  This  method  works  well  when  the  payload  (or  polygon)  is  small  and 


2 A  (’-inaiiifolii  is  a  manifold  in  a  configuration  space. 
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convex  in  a  relatively  uncluttered  obstacle  environment.  It  is  not  at  all  clear  how 
to  extend  the  algorithm  to  large,  non-convex  moving  objects,  or  how  to  consider 
more  than  one  rotational  degree  of  freedom  at  a  time.  Nevertheless  the  concept  of 
computing  “freeways,”  or  “channels”  through  free-space  is  attractive  in  that  it  can 
provide  global  guidance  to  local  algorithms  (such  as  C-Space  methods),  and  can 
enumerate  good  hypotheses  about  candidate  paths  through  complex  workspaces. 

Using  an  approach  called  retraction,  6’Dunlaing  and  Yap  (1082),  O'Dunlaing, 
Sharir  and  Yap  (1982)  construct  a  Voronoi  diagram  for  a  two-dimensional  workspace 
and  consider  moving  simple  objects  (a  disc,  a  line  segment)  along  it.  This  technique 
was  mentioned  by  Brooks  (1983a).  It  has  not  yet  been  extended  to  polygonal  objects 
or  3-dimensional  cases.  We  will  address  this  issue  by  considering  Generalized 
Voronoi  Manifolds. 

1.5.5.  Approximation  and  Completeness 

Planning  problems  have  two  components:  characterizing  the  constraints,  and 
searching  for  a  solution  which  satisfies  the  constraints.  One  attempts  to  achieve  a 
complete  (in  some  sense,  “exact”)  characterization  of  the  constraints,  and  a  complete 
search  algorithm  for  the  representation.  Since  the  Mover’s  problem  is  a  continuous 
mathematical  decision  problem,  we  must  in  general  consider  a  discretized  version  of 
the  problem  (sec  Reif  (1979)),  for  example,  we  might  represent  the  polyhedral  input 
models  as  systems  of  linear  inequalities  within  a  fixed  accuracy  e,  with  0  <  e  <  1. 
Ir.  fact,  there  are  two  kinds  of  resolution  limit,  Any  algorithm  which  employs  real 
arithmetic  has  a  resolution  limited  to  the  machine  precision.  (Schwartz  and  Sharir 
(1982a)  employ  rational  and  algebraic  numbers  instead).  For  the  find-path  problem, 
we  arc  interested  exclusively  in  the  physically  realizable  paths,  that  is,  mose  paths 
lying  entirely  within  open  sets  of  free  space.  The  resolution  limit  Reif  mentions  is 
essentially  a  bound  on  how  small  an  open  set  can  become  before  it  is  no  longer 
considered  open.  The  open  set  resolution  limit  is  typically  greater  than  the  machine 
precision. 

Almost  all  liud-path  search  algorithms  arc  complete  only  to  this  fixed  resolution; 
the  notable  exception  is  Schwartz  and  Sharir  (1982a),  which  appears  to  be 
search-complete  arid  resolution  independent.  We  should  stress  that  for  a  complete 


representation,  the  resolution-dependence  is  in  practice  not  a  severe  restriction. 
However,  the  effect  of  a  complete  search  algorithm  running  on  an  approximate 
characterization  of  the  constraints  is  not  clear.  In  principle,  in  case  of  search  failure, 
it  is  sometimes  possible  to  refine  the  approximation  and  redo  the  search  untii  a 
path  is  found.  This  possibility  has  rarely  been  exploited  however,  and  introduces  a 
number  of  unpleasant  technical  and  conceptual  issues.  A  complete  search  running 
on  an  approximate  representation  will  in  general  result  in  an  incomplete  algorithm. 
For  these  reasons  we  would  prefer  a  complete  characterization  of  the  constraints 
coupled  with  a  complete  search  algorithm. 

We  will  place  this  thesis  in  the  context  of  previous  work  by  considering  the 
following  criteria: 

(i)  For  what  degrees  of  freedom  does  the  algorithm  apply? 

(ii)  Is  the  representation  (the  characterization  of  the  constraints)  complete? 

(iii)  Is  the  search  complete  (at  a  given  resolution)? 

fivf  Has  Hin  nltrnriChm  hfx'n 

Approximate  Representations 

Much  of  previous  work  has  focused  on  approximate  characterizations  of  the 
constraints.  Approximate  representations  may  (i)  artificially  restrict  the  degrees  of 
freedom  in  a  problem,  (2)  bound  objects  in  real-space  by  simple  objects  such  as 
spheres,  or  prisms  with  parallel  axes,  while  considering  some  subset  of  the  available 
degrees  of  freedom,  (3)  discretize  configuration  space  at  certain  orientations,  or  (4) 
approximate  swept  volumes  for  objects  over  a  range  of  orientations.  Such  restricted 
planning  systems  may  lose  solutions  which  require  exploiting  all  six  degrees  of 
freedom.  An  approximation  of  the  obstacle  environment,  robot  model,  or  C- Space 
obstacles  can  result  in  a  transformed  find-path  problem  which  has  no  solution. 

Some  approximate  algorithms -for  example,  those  of  Brooks-run  quite  fast 
for  the  class  of  problems  that  they  address.  In  general,  speed  has  been  a 
motivating  factor  in  the  design  of  these  approaches.  Wc  also  observe  that  some 
approximate  methods  were  motivated  by  the  difficulties  of  modeling  constraints  in 
a  full  6-dimensional  C -Space..  These  difficulties  in  turn  stemmed  from  unresolved 
mathematical  problems  relating  to  both  C-Spacc  itself  and  to  the  structure  of 


C-Space  constraints.  However,  even  with  a  complete  mathematical  model  in  hand, 
we  are  still  confronted  with  the  problem  of  devising  a  complete  planner  which 
works  using  the  full  set  of  constraints. 

The  configuration  space  of  the  three  dimensional  classical  Movers’  problem  with 
six  degrees  of  freedom  is  9?3  X  50(3),  where  50(3)  denotes  the  three-dimensional 
rotation  group.  In  this  thesis  wc  first  complete  the  mathematical  framework  for  the 
configuration  space  3ft3  X  50(3)  and  present  solutions  for  some  heretofore  unsolved 
problems.  This  foundation  then  allows  us  to  propose  and  construct  a  complete 
planner  exploiting  the  full  set  of  constraints  and  6  degrees  of  freedom  for  motion 
planning  in  9ft3  X  50(3). 

In  this  section,  wc  characterize  the  completeness  of  previous  work.  Unless  noted, 
search-completeness  is  resolution-dependent.  Schwartz  and  Sharir  (1982a)  describe 
complete  representations  and  complete  (unimplcmentcd)  search  algorithms  for  2D 
and  3D.  These  theoretical  algorithms  appear  to  be  resolution-independent.  Drooks 
and  Lozano-Perez  (1983)  describe  complete  representations  and  search  algorithms 
for  the  problem  in  9ft2  X  51.  Lozano-Perez  (1981,  1983),  Lozano-Perez  and  Wesley 
(1979)  give  approximate  representations  (except  for  translation)  with  complete 
search  algorithms  for  9ft2  X  51  and  9ft3  X  50(3).  These  approximate  representations 
also  model  Cartesian  manipulators.  Most  algorithms  for  9ft3  X  50(3)  can  be  extended 
for  Cartesian  manipulators  in  a  similar  manner.  For  translations,  Lozano-Perez’ 
algorithms  are  complete  to  the  machine  resolution.  Drooks  (1983a)  provides  an 
approximate  constraint  characterization  with  a  complete  search  algorithm  for 
3ft2  X  51;  Brooks  (1983b)  extends  this  for  a  linked  arm  carrying  a  payload  with 
degrees  of  freedom  9ft3  X  5'.  A  significant  contribution  of  Drooks  was  the  addressing 
of  the  issue  of  jointed  arms.  The  open  set  resolution  limit  for  the  Voronoi  methods 
(for  simple  objects  in  two  dimensions)  is  no  larger  than  the  machine  precision. 
Udupa(l977)  and  Widdocs  (1974)  used  approximate  representations  and  incomplete 
search  algorithms  in  addressing  find-path  for  jointed  arms. 

In  this  light,  wc  can  characterize  our  algorithm  as  follows: 

•  This  thesis  presents  the  first  implemented,  representation-complete,  search- 

complete  algorithm  (at  a  given  resolution)  for  the  classical  Movers’  problem  in 


9J3  X  SO(3). 


1.6.  An  Outline  of  this  Thesis:  Research  Contributions 

In  this  thesis  we  present  a  local  algorithm  for  the  six  degree  of  freedom 
classical  Movers'  problem.  The  channel  based  algorithm  developed  in  Donald  (1984) 
is  described  in  Donald  (1983a). 

At  the  heart  of  this  research  lie  certain  mathematical  developments  that  may 
seem  fairly  abstract  at  first  reading.  To  motivate  the  mathematics,  we  first  present, 
in  chapter  2,  the  design  and  implementation  of  a  six  degree  of  freedom  planning 
system  for  the  classical  Movers’  problem.  The  description  of  the  planning  algorithm 
assumes  that  certain  representations  and  mathematical  tools  are  available.  In 
subsequent  chapters,  we  develop  these  tools  in  answer  to  the  following  questions, 
for  which  chapter  2  assumes  solutions: 


Representational  and  Algorithmic  Questions 


(i)  What  is  an  appropriate  representation  for  constraints  in  a  six  dimensional 
C-Space?  (Chapter  3). 

(ii)  In  the  six  dimensional  C-Space  of  the  classical  Movers’  problem,  the  domain  of 
each  constraint  is  the  product  space  of  and  a  complicated  three-dimensional 
manifold  (with  boundary)  on  the  projective  three  sphere.  What  are  these  regions, 
and  what  is  their  structure?  What  representation  can  be  used  for  these  domains? 
(Chapter  3). 

(iii)  IIow  do  we  plan  motions  using  constraints  whose  domains  change  with  the 
motions?  (Chapter  5). 

(iv)  Given  a  trajectory  in  C-Space,  it  is  necessary  to  find  where  it  intersects 
the  boundary  of  C~Spa.cc  obstacles.  IIow  can  trajectories  be  intersected  with 
C-surfaces  whose  domains  change  along  the  trajectory?  (Chapter  4,  5). 

(v)  IIow  can  intersection  manifolds  be  constructed  in  C-Space?  (Chapter  4). 

(vi)  IIow  are  motions  planned  that  slide  along  C-surfaccs  and  intersection  manifolds? 
(Chapter  4,  2). 

(vii)  IIow  can  rotation  space  be  decomposed  into  regions  where  the  set  of  applicable 
constraints  is  invariant?  (Chapter  5). 


How  to  Read  this  Thesis 


Chapter  2 — covering  the  design  and  implementation  of  the  search  algorithm  in 
C-Space — presents  the  most  heuristic  component  of  this  research.  It  is  also  in  some 
sense  the  most  accessible  chapter  to  the  non-speeDlist.  However  do  not  confuse 
chapter  2’s  implementation  details  and  search  heuristics  with  the  representational 
and  algorithmic  framework  developed  under  the  considerably  more  formal  aegis  of 
chapters  3  through  6.  The  thesis  is  structured  so  that  those  prefering  a  presentation 
more  in  keeping  with  the  traditional  styic  of  mathematical  exposition,  may  read  the 
chapters  on  GDOF  planning  in  the  alternative  order: 

(1)  Geometric  Planning  Problems 

(3)  Qu  estions  of  Representation:  C-functions  and  Applicability  Constraints  in  a 
Six  Dimensional  Configuration  Space 

(4)  Mathematical  Tools  for  Motion  Planning  in  a  Six  Dimensional  Configuration 
Space 

(55)  Moving  Through  Rotation  Space 

(G)  The  C-Voronoi  Diagram  and  its  Relationship  to  Intersection  Manifolds 

(2)  A  Planning  System  for  the  Classical  Movers’  Problem  with  Six  Degrees  of 
Freedom. 

In  the  alternative  order,  the  representations  and  algorithms  arc  derived  and 
presented  first,  and  the  application  and  implementation  is  presented  last. 

Chapter  3  presents  a  formal  framework  in  which  several  open  questions  about 
configuration  space  constraints — notably  (ii)  (above)  — may  be  solved.  Chapter  3 
also  derives  fundamental  structural  properties  of  C-Spacr.  constraints,  in  particular, 
the  domains  and  domain  topology  of  C-functions  for  the  classical  Movers’  problem. 
We  call  these  domains  applicability  constraints.  Chapter  4  addresses  the  intersection 
problem  in  high-dimensional  C- Space:  how  to  construct  and  slide  along  intersection 
manifolds,  and  how  to  intersect  trajectories  with  C-surfaces  and  applicability 
constraints.  We  demonstrate  the  form  of  the  intersection  manifolds  for  5ft2  X  S 0(3) 
and  5ft2  X  S'-  Chapter  5  discusses  algorithms  for  moving  through  rotation  space, 
and  for  decomposing  rotation  space  into  equivalence  classes  where  the  set.  of 
applicable  constraints  is  invariant.  In  chapter  {>,  we  extend  the  concept  of  the 
generalized  Voronoi  diagram  {which  Drysdule  (itJJ’liJ  iMur.-d  for  the  plane)  to  the  six 


dimensional  C-Space  8 ?3  X  50(3),  to  provide  a  formal  and  constructive  definition 
of  the  C-Voronoi  Diagram,  or  CVD.  The  CVD  is  an  attractive  construction,  in  that 
it  contains  a  representative  component  for  each  “branch”  of  free  space.  Each  such 
component,  is  a  submanifold  of  dimension  0  <  d  V  5,  called  a  Voronoi  manifold. 
We  will  derive  the  following  connection  between  intersection  manifolds  and  the 
CVD: 

Theorem:  ( The  Equivalence  Theorem  for  intersection  manifolds  and  the  CVD). 
Let  p  be  a  path  along  the  CVD.  p  lies  along  a  connected  chain  of  Voronoi 
manifolds,  Vj, . . . ,  V*.  We  demonstrate  that  for  each  Voronoi  manifold  V^,  there 
exists  an  equivalent  intersection  manifold  of  level  C-surfaces,  Furthermore, 
we  also  show  that  for  every  connected  chain  of  Voronoi  manifolds,  there  is  an 
equivalent  connected  chain  of  intersection  manifolds  (of  level  C-surfaccs).  (The 
equivalence  we  demonstrate  is  actually  stronger  than  homotopic  equivalence, 
but  the  additional  details  are  too  complicated  for  this  chapter). 


% 
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2 

A  Planning  System  for  the  Classical 
Movers’  Problem  with  Six  Degrees  of  Freedom 

In  this  chapter,  we  describe  the  design  and  implementation  of  a  planning 
system  for  the  classical  Movers’  problem  with  six  degrees  of  freedom.  The  planning 
algorithm  required  the  solution  of  the  seven  “Representational  and  Algorithmic 
Questions”  listed  at  the  end  of  chapter  1.  The  solutions  to  these  problems  are 
presented  in  subsequent  chapters. 

In  this  chapter  we  will  simply  assume  that  these  problems  arc  solved,  and 
proceed  to  employ  the  solutions  in  constructing  a  planning  algorithm.  Of  particular 
importance  will  be  two  effective  procedures,  which  address  the  intersection  -problem 
in  C- Space; 

(I)  Given  two  or  more  level  C-surfaces,  construct  their  intersection  manifold. 

(Chapters  3  and  4). 

(II)  Given  a  C-surfacc  and  a  trajectory,  Grid  their  intersection.  Determine 

whether  the  intersection  lies  on  the  boundary  of  a  C-Space  obstacle.  (Chapters 

4  and  5). 

The  immediate  application  of  (I)  is  the  sliding  problem:  How  to  slide  along 
one  level  C-surfacc,  and  how  to  slide  along  the  intersection  of  two  or  more  level 
C-surfaces. 
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Using  the  point  navigation  operators  (chapter  1),  wc  implemented  a  best-first 
search  algorithm  in  C-Space.  The  algorithm  has  nice  theoretical  properties  which 
include  completeness  (at  a  resolution).  This  chapter  describes  the  heuristic  search, 
with  particular  emphasis  on  the  heuristic  strategics  that  evaluate  local  geometric 
information,  and  on  the  interaction  of  these  strategies. 

2.1.  Definitions 


A  topological  space  M  is  called  an  ?a-diinensional  manifold  if  it  is  locally 
homeomorphic  to  5R”.  A  chart  is  a  way  of  placing  a  coordinate  system  on  M:  if 
U  and  V  are  open  subsets  of  M,  two  homcomorphisms  /  :  U  f(U)  C  ?Rn  and 
g  :  V  — *  g(V)  C  have  C°°  overlap  if  the  maps 

fog-l:g(Uf]V)^f(Uf\V) 
gof-'  :f(Uf)V)-*g{Uf)V) 

are  also  C°°  (that  is,  possessing  continuous  partial  derivatives  of  all  orders).  A 
family  of  pairwise  C^-overlapping  homcomorphisms  whose  domain  covers  M  is 
called  an  atlas  for  M.  A  particular  member  (/,  U)  of  an  atlas  U  is  called  a  chart  (for 
the  atlas  ll),  or  a  coordinate  system  for  U.  For  a  good  introduction  to  differential 
geometry,  see,  for  example,  (Spivak,  1979). 

In  this  thesis  we  usually  specify  charts  via  the  inverse  form  h  :  it  —*  M 
(where  R  is  an  open  subset  of  S?H)  with  the  understanding  that  it  is  the  inverse 
(or  set  of  local  inverses)  h~'1  which  provides  the  family  of  charts  { (/i-1,  VF;) },  for 
U,  VF,-  —  h{R).  As  an  example,  consider  the  map  h  that  specifies  a  chari  for  a  five 
dimensional  level  C-sui face: 


h  :  Si5  -v  IR'1  X  50(3) 

!  I  n  L\  t  E-zy  +  R\\z  +  —  t 

{y>z>  r,  o,  <p)  >->( - - - 

A  \ 


Here  the  Et  arc  smooth,  real-valued  functions  on  50(3),  that  is,  Ei  :  (ip,0,<p)  — > 
The  inverse  map  h~]  is  obvious,  and  provides  a  chart  for  the  live  dimensional 
submanifold  of  ltt'!  X  50(3).  In  subsequent  chapters  we  will  derive  such  charts,  in 
the  form  of  h ;  in  this  chapter,  we  will  take  them  for  granted. 


2.2.  Introduction 


We  are  now  ready  to  describe  a  planning  system  for  the  find-path  problem  in 
fft”  X  SO[3).  The  algorithm  has  the  structure  ol“  a  search  and  is  complete  (for  a 
given  resolution).  The  basic  idea  is  as  follows:  we  are  able  to  define  and  implement 
certain  local  operators.  When  applied  at  a  configuration  in  C-Space,  a  local  operator 
attempts  to  move  the  robot  in  a  specified  direction  until  cither  the  subgoal  or  an 
intervening  C-surface  is  reached.  The  local  operators  have  the  general  form 


M ove(X :conf  iguration,  it : direction ,  limit:conf  iguration), 


and  are  designed  to  return  X',  the  configuration  reached  in  direction  v,  and 
the  reason  for  stopping  (which  will  cither  be  “reached  subgoal’’  or  the  name  of 
the  C-surface  which  halted  progress).  The  local  operator  assumes  that  X  is  in 


free 


J  nnnii  »• 


aitu  v 


nsures  that  there  exists  a  collision-free  path  along  v  taking  the 


robot  from  configuration  X  to  X1.  Furthermore,  we  insist  that  limit  =  X  +  tv,  for 
some  positive  t.  Ir  general,  v  can  be  represented  as  a  tangent  vector  to  Ilf'1  X  £0(3); 
the  space  of  directions  is  clearly  locally  homeomorphic  to  Ilf6. 


Many  different  Move  operators  can  be  defined.  Let  X  =  (x,  0).  Wc  will  restrict 
v  to  be  cither  a  pure  translation 


v  £  Sf3  X  {  0  > 

or  a  pure  rotation 

v  G  {  +V’,  -i>,  + 0 ,  -0,  +4>,  -<}>  }. 

The  closure  of  these  operators  is  complete  for  the  space  of  configurations.  By 
this  we  mean  that  in  the  absence  of  obstacles,  there  is  some  finite  sequence  of 
operators  which  carries  any  configuration  X  into  any  other  configuration  Y .  It  is 
often  convenient  to  think  of  iliesc  operators  as  Translatc(X ,u,  x')  (where  u  £  3i,f 
and  x1  is  a  goal  translation)  and  Iiotatr.[X  (where  p  is  an  angular  direction 
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and  ip'  is  a  goal  angle).  The  theory  and  implementation  of  Translate  and  Rotate 
is  discussed  in  chapters  3,  4,  and  5. 


Given  the  local  operators,  we  can  define  more  sophisticated  local  strategics  for 
spatial  reasoning.  These  strategies  are  implemented  by  local  experts '  in  C-Space. 
For  example,  one  local  expert  attempts  to  circumnavigate  C-Space  obstacles  by 
sliding  along  inter?cc**ons  of  level  C-surfaccs.  Another,  “greedy”  expert  tries  to 
translate  or  rotate  straight  towards  the  goal.  A  local  expert  typically  examines  the 
local  geometric  environment  of  C-surfaces,  their  normals  and  intersections.  It  also 
takes  into  account  the  historj'  of  planning.  The  local  experts  can  be  thought  of  as 
issuing  “commands”  in  terms  of  the  local  operators.  Depending  on  the  results  of 
these  attempted  motions,  an  expert  may  issue  other  local  operator  commands,  and 
either  direedy  invoke  or  leave  a  forwarding  message  for  another  local  expert. 

I'o  summarize:  a  local  operator  is  ar.  algorithm  for  moving  along  a  specific 
trajectory  until  a  constraint  is  encountei  sd  (or  a  subgoal  is  reached)  .  A  local  expert 
is  a  strategy  for  choosing  the  trajectory  based  on  an  examination  of  the  history  of 
planning  and  the  local  geometry.  When  a  local  expert  chooses  a  trajectory,  it  calls 
on  some  sequence  of  local  operators  to  realize  it. 


'The  terns  lorul  expert  wus  brought  l.u  my  nU.entinn  in  disr.umiun*  willi  Van- Due  Nguyen  (Nguyen 
(HIM)),  Tonsils  l.ozune-lYre/,  nut!  liobiuy  liruoks. 
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Figure  2.1.  Schematic  illustration  of  the  “Bumble"  strategy  Jan  exhaustive  search).  A  fine 
six-dimensional  lattice  is  thrown  across  C-Space.  By  exploring  from  one  configuration  to  its 
neighbors  in  the  lattice,  a  path  will  eventually  be  found,  if  one  exists  at  the  lattice  resolution. 
Fortunately,  it  is  also  possible  to  take  large  steps  in  the  lattice,  and  simply  record  the  neighborhoods 
the  path  visits.  _ 

2.2.1.  Planning  and  Search 

The  planning  algorithm  is  implemented  as  a  search  o!  configuration  space.  The 
search  constructs  a  graph  of  neighborhoods  which  have  been  explored.  (We  will  be 
more  precise  about  the  term  neighborhood  later).  Each  node  in  the  search  graph  is 
associated  with  a  configuration  and  contains  information  about  the  local  geometry 
and  the  history  of  planning.  The  search  algorithm  chooses  a  node  for  exploration. 
Several  local  experts  are  then  applied  at  that  node.  Each  expert  can  produce  a  new 
search  node.  All  of  these  are  sons  of  the  explored  node,  and  arc  added  to  the  search 
queue.  The  new  sons  arc  connected  to  their  father  by  the  arcs  of  the  search  graph 
and  each  son  may  be  thought  of  as  an  exploration  from  the  father. 

If  at  any  point  in  the  search,  two  explorations  reach  the  same  neighborhood, 
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the  planner  attempts  to  merge  the  associated  nodes  into  one  node. 

The  search  algorithm  is  Best-First  (Nilsson  (1980))  with  the  metric  of  progress 
established  as  distance  from  the  goal.  (This  requires  placing  a  metric  on  both 
translation  and  rotation  space).  Other  search  measures  (such  as  path  length,  or 
time)  would  also  be  possible,  and  an  A*  search  strategy  could  be  exploited  to  find 
optimal  paths.  In  practice  this  would  probably  require  adding  new  local  experts  in 
order  to  ensure  reasonable  performance. 

At  search  nodes  arc  explored,  they  are  entered  in  a  priority  queue,  called  the 
search  queue.  The  nodes  in  the  search  queue  are  ordered  by  the  search  metric. 
Some  search  strategies  we  discuss  require  two  search  queues:  when  the  primary 
queue  is  exhausted,  then  nodes  from  the  reserve  queue  are  explored. 

We  will  proceed  as  follows.  First,  using  the  local  operators  alone,  we  can  define 
a  complete  search  strategy  (at  a  given  resolution).  This  search  strategy  can  be 
considered  the  most  primitive  local  expert,  and  is  known  as  the  ''Bumble  Strategy.” 
By  applying  the  Bumble  strategy  at  every  search  node,  we  arc  guaranteed  to  find 
a  path  (at  a  given  resolution)  if  one  exists. 

Next,  wc  will  define  more  complicated  local  experts  which  will  be  applied  to 
search  nodes  at  the  same  time  as  the  Bumble  expert.  These  experts  greatly  improve 
the  performance  of  the  planner. 


2.3.  A  Complete  Search  Strategy 


A  search  node  is  associated  with  a  configuration.  Every  configuration  is  in  turn 
associated  with  a  neighborhood  of  C-Space.  The  neighborhoods  form  a  partition 
of  C-Space.  Since  many  configurations  are  associated  with  one  neighborhood,  so 
several  search  nodes  may  have  configurations  lying  in  the  same  neighborhood. 

Assume  the  neighborhoods  are  “small.”  If  the  configurations  of  two  search 
nodes  are  in  the  same  neighborhood,  it  indicates  that  they  should,  if  possible,  be 
merged  into  one  node,  since  they  are  close  together.  By  keeping  track  of  the  set  of 
explored  neighborhoods,  we  can  avoid  redundant  explorations.  If  the  neighborhoods 
arc  sufficiently  small,  then  the  search  will  be  complete  at  a  resolution  closely  related 
to  the  neighborhood  size. 
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Figure  2.2.  f,g  :  R3  X  50(3)  — *  SC  arc  C-func.tiona  which  describe  two  level  C-surfaces,  ker / 
and  ker  3.  The  love!  C-surfaces  arc  smooth,  5-dimensional  manifolds  parallel  to  C-Space  obstacle 
boundaries.  From  X  €  ker/,  three  paths  sliding  along  the  level  C-surfacc  ker/  are  shown. 
Each  path  is  orthogonal  to  V/.  The  sliding  expert  plans  such  paths  along  5-D  level  C-surfaces. 
(ker  jf)  rK^cr  (7)  is  the  intersection  of  the  two  level  C-surfaces,  and  is  a  -l-diincnsional  manifold. 
The  intersection  expert  plans  paths  along  intersection  manifolds.  Such  a  path  p  is  shown  from 
configuration  Y . 

It  is  possible  to  devise  a  complete  search  strategy  (at  a  given  resolution)  using  just 
the  iocai  operators.  We  first  throw  a  fine  six-dimensional  lattice2  over  configuration 
space.  The  lattice  is  used  to  keep  track  of  the  state  of  the  planner,  i.e.,  which 
neighborhoods  have  been  explored,  and  for  computing  the  connectivity  of  these 
neighborhoods.  The  lattice  will  “wrap  around”  in  the  rotational  dimensions,  hut 
this  is  easily  implemented  using  modular  arithmetic.  We  will  define  an  adjacency 
function  for  points  in  the  lattice;  in  addition,  when  a  neighborhood  is  explored, 
the  corresponding  node  in  the  lattice  is  marked.  When  a  search  node  is  chosen  for 
exploration, 


2I,c.,  the  factor  spaces  or  tlic  parameter  space  arc  quantized,  and  the  lattice  is  a  partial  order 
on  the  Cartesian  product  of  the  factor  space  quantizations. 


(i)  X ,  the  configuration  of  the  search  node  is  mapped  to  L,  a  point  in  the  lattice. 
L  is  the  name  of  the  neighborhood  X[L)  centered  on  L,  which  contains  X. 

(ii)  The  unexplored  neighborhoods  adjacent  to  N{L)  are  found.  Kacli  of  these 
neighborhoods  is  also  identified  by  a  central  lattice  point. 

(iii)  The  planner  attempts  to  move  to  each  of  the  unexplored,  adjacent  neighbor¬ 
hoods. 

(i)  has  the  effect  of  mapping  a  neighborhood  of  C-Space  to  a  canonical  element 
{which  lies  on  the  lattice)  in  its  interior.  These  neighborhoods  decompose  5R3  X  ■50(3) 
into  equivalence  classes  with  the  same  canonical  clement.  When  a  neighborhood 
is  reached  for  the  first  time,  we  mark  its  lattice  point  as  explored.  The  search 
terminates  when  a  neighborhood  containing  the  goal  is  reached,  and  when  that 
exploration  can  be  connected  to  the  goal  configuration. 


2.3.1.  Implementation  of  Neighborhoods  and  Lattices 

In  principle,  it  is  possible  to  implement  the  lattice  as  a  six-dimensional  array 
(with  modular  indexing  for  the  rotational  dimensions).  In  practice,  for  any  fine 
resolution,  this  array  will  be  enormous,  and  very  sparse.  Although  an  adversary 
can  design  a  find-path  problem  for  which  our  planner  must  explore  the  entire 
lattice,  in  practice  this  does  not  occur.  However,  we  must  maintain  a  record  of  what 
neighborhoods  have  been  explored,  in  order  to  generate  the  unexplored  neighbors 
for  a  search  node.  Since  the  array  is  sparse,  we  will  employ  a  different  strategy. 

A  partial  order  can  be  defined  on  lattice  points  by  considering  them  as 
six-dimensional  vectors.  This  order  has  no  particular  geometric  significance  for 
the  rotational  dimensions,  but  it  can  bo  used  to  store  explored  lattice  points  in  a 
binary  tree.  Since  the  vast  majority  of  neighborhoods  are  never  explored,  the  tree 
it  typically  small,  even  for  fine  lattices.  To  mark  a  lattice  point  as  explored,  we 
insert  into  the  binary  tree.  To  find  whether  a  lattice  point  has  been  explored,  v/e 
search  the  tree. 

It  is  desirable  to  employ  a  fine  lattice  in  order  to  ensure  completeness  at  a  fine 
resolution.  The  use  of  a  binary  tree  to  record  explored  configurations  effectively 
removes  the  problem  of  lattice  size  for  storing  explored  configurations.  For  example, 
if  wc  segment  C-Spacc  into  an  N  X  N  X  • . .  X  N  lattice,  then  an  array  would 
have  to  be  N long.  But  the  binary  tree  need  store  only  the  explored  locations,  and 
(if  height-balanced)  can  access  any  leaf  in  O(log  N)  operations. 
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If  the  lattice  resolution  is  fine,  then  the  planner  as  described  so  far  will  take 
very  small  steps  for  each  search  exploration.  This  has  been  remedied  as  follows: 
If  a  local  operator  is  invoked  to  find  whether  limit  may  be  attained  from  X  in 
direction  v,  it  must  effectively  intersect  a  path  in  direction  v  with  all  C-surfaccs.  It 
is  not  much  harder  to  find  the  first  constraint  along  the  path  p(i)  =  X  +  tv  (even 
if  it  is  beyond  limit):  in  particular,  we  note  that  all  intersections  along  the  path  p 
may  be  sorted  on  distance  from  X .  The  complexity  of  finding  this  first  intersection 
along  p  is  independent  of  the  lattice  resolution  (since  the  intersection  algorithm  has 
nothing  do  do  with  the  lattice;  sec  chapter  5).  We  can  “sample”  the  portion  of  the 
path  which  lies  in  free  space  at  the  lattice  resolution.  All  of  these  configurations 
are  then  marked  as  “explored”,  and  as  reachable  from  their  immediate  neighbors 
along  the  path.  Thus  they  form  a  connected  chain  in  the  lattice  along  the  path  p. 
While  all  these  configurations  are  in  some  sense  sons  of  X,  in  practice  we  will  select 
only  one  or  two  to  be  entered  in  the  primary  search  queue.  These  sons  might  be 
(l)  the  son  v/hich  is  closest  to  the  goal,  and  (2)  some  son  at  a  reasonably  large  step 
away  from  X .  This  step  size,  called  the  Bumble  resolution,  might  be  3  to  10  times 
the  lattice  resolution.  The  other  sons  should  be  kept  on  a  reserve  queue,  which  can 
be  explored  when  the  primary  search  queue  is  depleted  or  exhausted. 

In  practice,  it  may  preferable  to  enter  ranges  in  the  exploration  tree,  for 
example,  to  record  that  all  lattice  points 

(x,y,z,i!>,0,<f>)  <  L  <  [x  4-  kdr,y,z,il),0,<t> ) 

(for  some  integer  /c)  are  explored.  This  requires  keeping  an  exploration  tree  of  lines 
instead  of  configurations,  with  the  intent  of  minimizing  the  number  of  exploration 
tree  entries.  When  lines  arc  entered  into  the  tree,  they  may  be  merged  with  previous 
lines  to  form  connected  components  of  explored  regions.  These  operations  are 
supported  by  hierarchical  subdivision  algorithms.  At  this  point  in  the  experimental 
use  of  the  planner,  it  is  still  too  early  to  tell  whether  this  optimization  is  necessary. 
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In  practice  we  have  had  no  problem  in  selecting  a  very  fine  resolution  for 
the  lattice  (one  selects  a  fine  lattice  resolution,  and  a  considerably  larger  Bumble 
resolution  or  step  size,  as  described  above).  This  lattice-based  strategy  is  not 
only  theoretically  complete  for  a  given  resolution,  but  has  also  been  used  to  find 
very  complicated  paths  for  the  ti  degree  of  freedom  classical  Mover’s  problem. 
However,  the  algorithm  has  an  “excessively  local”  flavor — it  is  clumsy  and  quite 
slow  when  employed  alone  (hence  the  strategy’s  name).  We  can  construct  much 
“smarter”  heuristic  experts  which  attempt  to  exploit  coherence  in  C-Space.  When 
these  experts  arc  used  in  conjunction  with  the  Bumble  strategy,  wc  obtain  a 
planner  which  is  not  only  complete,  but  which  can  solve  complicated  problems  in 
a  reasonable  amount  of  time.  We  continue  to  find  the  lattice  useful  for  recording 
the  planner’s  explorations  by  the  local  experts. 


2.3.2.  Keeping  Track  of  Connectivity 

Suppose  a  subsequent  exploration  reaches  the  same  neighborhood.  There  are 
two  choices,  which  wo  call  the  tniii K  aigOi lciiiu  auo  cue  conned  aigoriintn; 


The  Mark  Algorithm.  Discard  the  exploration,  since  the  neighborhood  is  already 
explored.  In  practice,  the  mark  algorithm  often  suffices  Tor  path-finding.  The  mark 
algorithm  computes  a  directed,  spanning  tree  T  of  explored  neighborhoods,  which 
is  rooted  at  the  start  configuration. 


The  Connect  Algorithm.  Connect  together  the  search  nodes  for  all  explorations  to 
that  neighborhood.  The  connect  algorithm  is  more  complicated,  and  requires  the 
following  bookkeeping  (see  figure  2.3).  Let  M  be  a  neighborhood  of  X  £0(3), 
and  L  (E  i[M )  be  a  lattice  point  which  is  the  canonical  element  for  M .  Suppose  X  is 
an  exploration  of  M ,  i.c - ,  X  C  U  is  the  final  configuration  in  some  motion  reaching 
M.  Let  s(X)  denote  the  search  node  for  X.  (If  X  is  the  first  exploration  of  A/,  then 
create  a  search  node  s[L)  for  /,).  Determine  whether  there  exists  a  path  from  X  to 
L  (using  the  local  operators).  If  so,  connect  s(X)  and  s(/,)  together. 


The  connect  algorithm  computes  a  m  >rc  complete  connectivity  graph  for 
the  neighborhoods  of  C-Spacc.  It  computes  an  undirected  graph  11  of  explored 
neighborhoods,  which  may  contain  cycles.  As  long  as  //  is  connected,  then  T 


Figure  2.3.  The  lattice  point.  L  is  at  the  center  of  a  neighborhood  M  of  C-Syat.t.  Search 
explorations  arrive  at  configurations  X  and  Y  in  M.  The  planner  attempts  to  find  a  path 
connecting  X  and  V,  by  trying  to  connect  both  configurations  to  L. 


is  a  spanning  tree  for  II,  and  the  mark  algorithm  is  complete  for  planning  a 
connected  path  along  II.  However,  not  all  planning  strategies  admit  this  kind  of 
“connected  planning.”  In  particular,  when  we  consider  strategies  which  construct 
partial  paths  auu  planning  islands  (which  may  later  connect  up),  the  connect 
algorithm  is  necessary.  (See  the  Suggcstor  strategy,  below,  for  an  example). 


2.3.3.  Discussion  of  the  Bumble  Strategy 


Suppose  the  lattice  spacing  is  dj-  and  djt  in  the  translational  and  rotational 

dim 


ion-..  1  hen  the  adjacent  lattice  points  to  L  —  (z,y ,  z,\4,0,  tf>)  will  be: 


(x  ±dr,y,z,il>,0,^) 

{x.y  ±  dT>z,\b,0,4) 

(x,y,z±  dr,  V,  0,4) 

{*,  v,  Z,  */’  ±-  du  (mod  2tt),  5,  0) 

(i,  V,  z,  V, 0  ±  du  (mod  2 tt),  4) 

( I ,  V;  V'l 4  :t  (mod  2ft)) 

Each  adjacent  lattice  point  is  the  center  of  a  neighborhood  of  configurations  which 
is  contiguous  to  the  neighborhood  of  L.  Each  such  neighborhood  can  be  reached 
(if  it  is  in  free  space  and  there  is  no  intervening  C-surfacc)  by  the  local  operators 
Translate  and  RoiaLc.  Since  there  arc  12  neighbors  for  each  lattice  point,  wc  have 
found  it  inadvisable  to  explore  them  all  for  each  search  node  expansion..  Instead, 
the  set  of  unexplored  adjacent  neighborhoods  is  ranked  (in  terms  of  proximity 
to  the  goal),  and  motions  towards  the  top  ky  translational  and  kjt  rotational 
neighbors  arc  attempted.  (Typically,  k-r  ~  3  and  kti  «  2).  If  the  node  is  recxplored 
later,  motions  toward  ky  H-  ky  more  of  the  unexplored  neighbors  will  be  attempted 
(if  there  are  that  many  left).  When  using  the  mark  algorithm  (above),  we  say 
an  exploration  is  successful  if  it  reaches  a  new  (unexplored)  neighborhood.  If  an 
exploration  is  successful,  then  a  new  search  node  is  created  and  the  neighborhood 
is  marked  as  explored.  Since  the  neighborhood’s  “name”  is  its  lattice  point.,  this 
simply  corresponds  to  marking  the  lattice  point.  Whether  successful  or  not,  all 
explorations  are  recorded  at  the  parent  scaich  node  so  that  they  will  not  be  tried 
r-gain. 
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point  f. .  The.  uriexphned  adjacent  lattice  points  to  L  indicate  a  set  of  .subgoals  to 
be  attained  from  X.  The  Hembh-  strategy  ranks  these  subgoals,  chooses  some  of 
them,  :md  sc  Vets  trajectories  which  may  attain  them.  The  local  operators  arc  then 
employed  to  (try  to)  realize  the  selected  trajectories.  These  explorations  arc  then 
recorded  sc  that  only  ncs/  explorations  will  be  pursued  in  the  future.  Note  that 
the  pi.uuici  is  not  constrained  to  move  along  the  lattice,  and  that  although  the 
subgoals  lie  on  the  lattice,  the  motion  from  X  to  any  subgoa!  docs  not,  unless 

X  --  L. 


The.  local  experts  are  considerably  more  sophisticated  than  the  Humble  strategy. 


Their  subgoals  need  not  I  in  on  the  lattice,  and  the  motions  specified  to  the  local 
operators  need  not  lie  along  the  lattice.  The  lattice  is  still  employed  tc  keep  track 
of  the  planning  history  and  the  connectivity  of  explored  neighborhoods. 

Clearly,  the  arcwisc-ccunectcd  sets  of  lattice  points  are  closed  under  the 
operators  Translate  and  Rotate.  If  a  path  exists  at  the  lattice  resolution,  then 
the  search  is  guaranteed  to  find  it.  Wc  sec  now  exactly  what  the  resolution  for 
this  find-path  algorithm  is:  by  choosing  a  sufficiently  fine  lattice,  the  algorithm  is 
(tri  daily)  complete  at  the  lattice  resolution.  As  wc  saw  above,  we  can  choose  a  very 
fine  lattice  with  little  computational  overhead.  One  final  point:  the  start  and  goal 
configurations  may  not  lie  directly  cn  the  lattice.  This  is  not  a  problem,  however, 
since  the  local  operators  can  ensure  that  there  exists  a  path  from  the  start  and  goal 
to  the  nearest  iatticc  point. 

2  A.  Local  Exper  ts  for  the  Find- Path  Problem 

2,4,1.  Path  Planning  versus  Continuous  Intersection  Detection:  Why  We 
Need  Local  Experts 

The  Translate  and  Rotate  operators  detect  collisions  along  continuous 
trajectories.  Civeii  these  operators,  it  is  possible  to  devise  a  complete  path-planning 
algorithm  based  on  something  like  the  Bumble  strategy,  above.  However,  while 
complete,  this  is  not  a  particularly  good  algorithm,  in  that  it  says  nothing  about 
how  or  when  the  operators  should  be  applied.  The  domain  of  the  operators  is 
large  r.i.d  lor  realistic  path  planning,  it  is  necessary  to  know  where,  and  in  what 
directions  to  apply  them. 

Algorithms  which  can  delect  intersections  with  obstacles  for  a  robot  following 

a  continuous  trajectory  say  nothing  about  how  to  plan  these  trajectories. 

However,  the}  can  be  used  to  find  a  path  by  exhaustive  search. 

The  Translate  and  Rotate  operators  use  the  constraints  in  C- Space  to  detect 
collisions.  However,  these  constraints  can  also  be  employed  to  plan  paths.  In 
chapter  i,  we  proposed  an  idealized  planner  which  constructed  the  intersection 
3 Tii IS  liiHcii.Ksiiin  hull}  ,  fur  I  In-  £i-m  ral  Move  operator. 


manifolds  of  level  C-surfaecs,  and  slid  along  these  manifolds  to  navigate  a  cund 
C-Space  obstacles.  Such  a  planner  could  exploit  coherence  in  configuration  space: 
by  examining  C-Space  constraints  an  algorithm  can  be  devLed  for  intersecting 
and  sliding  on  C-surfaces  to  circumnavigate  C-Space  obstacles.  In  the  following 
sections,  we  describe  a  planner  which  approaches  the  idealized  planning  algorithm 
of  chapter  1.  The  local  experts  are  strategies  for  reasoning  about  the  local  geometry 
of  configuration  space,  and  for  exploiting  geometric  constraints  to  plan  collision-free 
paths.  When  applied  to  a  search  node,  each  local  expert  examines  the  local  geometry 
and  history  of  planning  to  propose  one  or  more  path  segments.  Each  path  segment 
is  realized  by  means  of  the  local  operators,  which  ensure  that  a  collision  free  path 
exists. 

2.4.2.  Designing  Local  Experts 

In  the  exploration  tree  of  C-Space  neighborhoods,  we  have  seen  one  type  of 
information  that  must  be  maintained  for  planning.  In  designing  local  experts,  we 
must  address  the  following  questions: 

(i)  What  constitutes  a  local  description  of  a  (level)  C-surface? 

(ii)  What  information  should  be  stored  at  a  search  node? 

(i)  can  be  stated,  ‘'What  constitutes  a  sufficiently  rich  description  of  the 
local  geometry  in  C-Space  to  allow  robust  local  experts?”  (ii)  relates  more  to  the 
history  of  planning,  and  the  connectivity  oi  the  explored  search  neighborhoods. 
For  example,  we  want  to  record  the  results  of  previous  applications  of  experts  at  a 
search  node,  arid  the  adjacent  nodes  in  the  search  graph. 

The  Local  Description  of  a  C-surface 

A  C-surface  has  a  normal  at  point  A".  Motions  tangent  to  the  C  surface  at  X 
will  have  instantaneous  velocities  orthogonal  to  the  normal.  We  must  characterize 
the  normal  and  tangents  to  a  C-surfacc  in  order  to  plan  trajectories  which  slide 
along  it. 

Let  /  be  an  applicable,  positive- valued  C-function  at  X .  We  can  check  that  /  is 
non-redundant  at  X  (sec  chapter  6);  alternatively,  we  may  heuristieally  assume  /  is 
non-redundant  if  its  value  at  X  is  small.  We  wish  to  develop  a  local  characterization 


of  /  at  X,  that  is,  of  the  level  C-surface  -S'  =  {  Y  j  f(Y)  —  f(X)}  about  X .  We 
should  think  of  S  as  the  kernel  of  the  auxiliary  function 


fx  :  X  50(3)  -  St 

y  -*•  m  ~  An 

The  local  characterization  wiU  have  two  parts,  one  of  which  is  invariant,  and  one 
of  which  will  change  for  different  subgoals,  The  invariant  part  of  the  description  is 
a  pair, 


(fm  v/) 


consisting  of  the  value  of  /  at  X  and  the  normal  to  S  at  X.  Now,  since  S?3  X  50(3) 
is  not  a  vector  space,  the  normal  V/(X)  to  5  at  X  will  depend  on  the  Riemannian 
metric  defined  on  the  tangent  space  at  X.  We  will  employ  a  metric  which  admits 
construction  of  V/(X)  using  the  partial  derivatives  of  /  at  X,  with  respect  to  the 
parameterization  of  C-5pace.  Hence  if  rotations  are  parameterized  by  Euler  angles, 

then  Vf  ==  (&  ‘V-  ‘21  U  ‘ll\ 
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Assume  that  V/  is  normalized  to  be  a  unit  vector.  We  i  ov/  wish  to  characterize 
the  relationship  of  the  C- surface  to  some  subgoal,  G :  this  requires  some  way  of 
talking  about  directions  in  Di*  X  50(3).  Specifically,  we  wish  define  a  “vector’' 
algebra  on  configurations,  such  that 


and 


lim.  ||G-Jf||  =  0 

G  — *  A 


(a‘  -x)  —  l. 


These  equations  express  the  vector  space  characteristics  which  are  required  for 
our  computations  on  tangent  vectors.  To  construct  this  algebra,  it  is  possible  to 
define  a  field  of  inner  products  over  9?'*  X  50(3),  i.e.,  to  define  an  inner  product  on 
the  tangent  space  to  each  point.  Thus  X  50(3)  is  a  Riemannian  manilold  (see 
Erdmann  (1084)).  If  two  tangent  vectors -i.e.,  directions -are  applied  to  the  same 
point,  this  inner  product  allows  us  to  talk  about  the  angle,  between  two  such  tangent 
vectors,  or  of  the  angle  between  an  arbitrary  tangent  vector  to  9i  *  X  50(3)  and  the 


normal  to  a  C-surfacc.  However,  the  inner  product  is  somewhat  arbitrary  for  our 
application  Alternatively,  we  could  also  construct  geodesics  on  /v,l  the  3-sphere 
witn  antipoda1  points  identified.  These  approaches  are  probably  too  elaborate  for 
a  heuristic  strategy. 

Heuristics  for  Evaluating  Directions  in  the  Tangent  Space 

A  basic  issue  is  that  placing  a  metric  on  a  non-abelian  group,  such  as  50(3), 
is  a  difficult  problem.  VVc  will  demonstrate  the  metric  that  our  planner  employs, 
and  then  show  that  it  is  adequate  for  this  application.  In  particular,  the  metric 
is  adequate  when  applied  to  three  one-dimensional  slices  of  50(3).  (These  are  the 
slices  considered  by  the  Rotate  operator).  Note,  however,  that  a  metric  may  also 
be  derived  by  representing  rotations  as  unit  quaternions.  In  this  case,  the  metric  is 
obtained  by  considering  rotations  as  points  on  6*3  embedded  in  (Brou  1983). 

Suppose  we  employ  rotation  matrices  to  represent  rotations.  (The  implemented 
planner  uses  Euler  angles).  If  we  are  willing  to  tolerate  singularities  in  the 
representation,  it  is  often  convenient  to  identify  a  rota' ion  matrix  in  50(3)  with 
the  vector  of  three  angles,  (»/’,  0,  <p)  which  determine  it.  The  angles  form  a 

three  dimensional  angle  spare,  Q 3.  The  rotation  matrix  corresponding  to  (i p,6,4>)  is 
of  course  Z(ip,d,  </>).  (The  singularities  induce  an  equivalence  relation  on  Q  ,  vdiere 
two  points  in  angle  spare  are  equal  when  the  rotation  matrices  they  determine 
arc  equal).  Most  of  the  time.,  the  identification  of  50(3)  with  Q does  not  lead 
to  problems.  However,  when  wo  wish  to  compute  directions,  and  •differences  of 
configurations,  it  is  necessary  to  distinguish  between  50  (3)  end  Q' . 

Wc  can  state  this  more  concisely  as  follows:  50(3}  is  a  three  dimensional 
manifold.  The  mapping  M  from  Eu’icr  angles  to  rotation  matrices  is  a  chart  for 
50(3): 

R  :  Q*  -*  SO{b). 

Wc  typically  describe  a  rotation  £(()}  G  50(3)  by  its  chart  coordinates  ( xl>,0,<j> )  — 
0  £  Q{.  This  makes  it  convenient  to  identify  0  with  ,V(0),  so  that  in  general, 
instead  cf  dealing  with  the  manifold  directly,  wr  will  work  with  a  chart  for  the 


manifold,  In  this  section  alone,  however,  we  must  distinguish  between  the  domain 
and  image  of 

We  can  compute  a  direction  in  Di'*  X  50(3)  by  simply  subtracting  two 
configurations  (of  course  the  angles  must  be  subtracted  (mod  27r))  to  yield  a 
six-dimensional  direction  vector.  Using  this  arithmetic,  the  goal  directionis  denoted 
G  —  X.  We  will  use  the  convention  that  the  first  three  coordinates  of  G  —  X  arise 
from  9J3,  and  the  second  three  coordinates  arise  from  Q 3. 

Let  G  =  (GxiGq)  and  X  —  (Xx,Xq).  Since  G  —  X  is  clearly  well  defined 
when  G  and  X  differ  only  by  a  translation,  assume  that  G  and  X  differ  only  by  a 
rotation.  Assume  further  that  rotations  are  represented  by  Euler  angles.  Note  that, 
in  general  G  —  X  is  not  a  rotation  which  carries  the  moving  object  at  orientation 
G  into  the  moving  object  at  orientation  X.  However,  G  —  X  does  represent  the 
difference  in  orientation,  i.c.,  it  specifics  a  displacement  in  angle  space  which  will 
carry  G  into  X.  For  example,  if  Gq  =  (45°,  5G'  ,  $0°)  and  Xe  “  (45°,  45°,  45°)  then 
there  arc  rotation  matrices  x(<?t!)  and  .£( A^)  corresponding  to  each  of  Gq  and 
Xf )■  (Wc  use  degrees,  not  radians  in  this  example,  since  the  symbol  n  will  soon  be 
used  for  a  projection  map).  Note  that 


je(45°, 50°, 9G°)  ^  £(45°,  45°,  45°)£(0°,  5°, 45°), 
where  R.R.1  indicates  composition  of  rotations.  However,  the  path  in  angle  space 

p(£)  =  +  1(Gq  --  J^q) 

=  (45°,  45°,  45°)  -r  i(0°,  5°,  45°) 

(for  t  £  [0,  1])  will  work,  since  it  corresponds  to  the  rotational  path 


£(p(0)  -  R(Xe  +  £(Ge  -Xe)) 

=  £((45°,  45°,  45°)  +  5°,  45°)). 

Considering  configuration  space  as  the  product  space  of  the  translation  space 
and  the  angle  space,  we  sec  that  G  —  X  is  wel.  .Mined-  u’s  —  A  3  specifics  a 


direction  and  a  distance  to  be  traveled  in  angle  space  in  order  to  carry  X&  into  G&. 
Furthermore,  along  t.hc  patli  f;or/i  Xy  to  Gy,  the  corresponding  rotations  specified 
by  the  angle  space  trajectory  j>  are  well  defined.  For  all  G  G  X  SO{ 3),  wc  will 
treat  the  space  of  directions  G  —  X  as  the  tangent  space  T\-  to  S?'{  X  S0(3)  at  X. 
Properly,  ‘l\-  is  the  product  space  of  the  tangent  space  to  3J'1  at  Xt,  and  the  three 
dimensional  angle  space  QJ. 

We  now  define  a  map  from  Ty  X  Ty  to  the  plane,  which  will  function  in  place 
of  an  inner  product.  Fiist,  define  the  natural  projection  maps  from  7\  onto  its 
factor  spaces: 


7fj}3  :  Tx  9J3 

(G-X)~[Gx-Xx) 

:  l'x  — ►  Q 3 

(G-X)  »-(Ge-X0). 


Let  ti  ■  v  denote  the  standard  inner  product  on  Dr,  for  vectors  u  and  v.  If  u 
and  v  are  projections  (under  7^3)  of  direction  vectors  in  Tx,  we  say  that  u  and  v 
arc  tranalalionaliy  orthogonal  if  u  ■  v  —  0.  Let  ( q\ ,  q>,  q:\),  (toi,  ugj,  103)  G  Assume 
the  each  pair  of  angles  g,  and  to,-  (for  t  =  1,2,3)  is  normalized  so  that 

|<7,  —  tu,j  <  180°. 

(Nolo  that  this  normalization  is  critical).  Now,  define 

?i<m9l>92,93),(wi>™Z,W3))  =  q\W\  +  q2™2  +  <?3™3- 


np  will  function  in  place  of  an  inner  product  on  Q  *.  We  say  that  two  rotational 
directions  q  and  w  are  rolationally  orthogonal  if  ?iy(«7,  to)  —  0. 


We  may  now  define  <1>A',  which  will  function  in  place  of  an  inner  product  on 
7\- ■  First,  let 

D  —  G  —  X 
D’  —  G'  —  X . 


Hi 


Assume  that  Dx,  D$,  ami  D&  are  all  normalised  to  be  length  1  (where  the 
length  of  Dq  is  defined  as  nq'D#,  D^)J ).  Finally, 

$x  :  Tx  X  Tx  ->  ft2 

(l>,  D')  ->  ^(V)  ■  *«,(/>'),  ^(nep),  Tro(O')))- 

So  yields  a  pair  consisting  of  the  dot  product  of  the  translational  components 
of  the  direction  vectors,  and  the  nq  product  of  the  rotational  direction  vectors.  If 
®x(D,  D')  —  (0,0),  we  say  that  D  and  D'  are  orthogonal  directions  in  the  tangent 
space  Tx-  Note  that  two  directions  are  orthogonal  if,  and  only  if,  their  translational 
components  are  orthogonal  and  their  rotational  components  are  orthogonal. 

This  discussion  extends  naturally  to  other  representations  for  rotations.  For 
example,  if  spherical  angles  (Kane  and  Levinson  (1978))  are  used,  then  the  difference 
in  orientation  is  the  rotation  carrying  G  into  X,  that  is,  G&  —  X(j  ie  a  rotation 
carrying  the  moving  object  at  orientation  Xq  into  the  moving  object  at  orientation 
G’q.  We  should  stress  that  the  natural  Kiemannian  inner  product  (Erdmann  (1984)) 
could  be  used  instead  of  <!>*.  This  would  complicate  the  representations  employed 
in  subsequent  chapters.  Q>x  and  uq  arc  heuristic  measures  on  directions  in  Tx -  We 
will  later  discuss  why,  for  our  purposes,  they  are  good  heuristic  measures. 

Evaluating  Normals  and  Gradients  to  C-Surfacea 


The  local  description  of  a  C-surfacc  relative  to  some  subgoal  is  designed  to 
address  the  following  qualitative  questions: 

(i)  is  the  C-surfacc  iocaiiy  tangent  or  locally  orthogonal  to  the  goal  direction? 

(ii)  Is  the  C-surface  locally  orthogonal  to  any  rotational  motion? 


Recall  that  a  level  C-surface  kcr  /  is  described  by  a  real  valued  C-function  /. 
Assume  that  normals  and  tangent  vectors  are  appropriately  normalized.  Question 
(i)  ma,y  be  resolved  by  examining 


*;v((C-n  V/( X)). 


Wlic-n  (?.l)  approaches  (0,  (L,  w<  say  that  kcr  f  is  locally  tangent  to  the  goal  direction. 
Note  that  (2.1)  makes  .sense,  f  maps  parameters  of  the  form  [ x,y ,  ,  0,  ip)  to  real 


f 

j 


numbers,  and  hence  the  gradient  of  /, 

(LI  di  dA  d±  dl\ 

\  dx  ’  dy  ’  dz  ’  drj} '  dO  ’  d<j> ) 

is  clearly  a  direction  in  Tx- 
We  will  also  employ 

^(G-X).7rR:,(V/(X)),  (2.2a) 

When  (2.2a)  approaches  0,  wc  say  that  ker  /  is  (locally)  translationally  tangent 
to  the  goal  direction.  Symmetrically,  when  (2.1)  (rer>p.  (2.2a))  approaches  (1,1) 
(resp.  1),  we  say  that  ker  /  is  locally  orthogonal  (resp.  translationally  orthogonal) 
to  G  —  X.  A  similar  calculation  yields  the  rotationally  tangent  and  orthogonal 
C  surfaces  to  the  goal  direction: 

nQ{^s{u  ~X),  7T0(V/(X))).  (2.26) 

Why  and  nq  are  Good  Heuristic  Measures 

Suppose  that  the  rotational  direction  is  along  one  of  the  axes.  (Let  us  say  the 

A 

direction  is  4>).  To  tell  whether  a  C-surface  is  rotationally  orthogonal  (or  tangent) 
to  the  4>  direction,  we  simply  examine  the  magnitude  of  which  can  be  obtained 
directly  from  V/(X).  This  is  because 

nQa,  "warn  =  ««((o.o.  0,  %.%))  =  %■ 

In  other  words,  the  map  tiq  need  not  be  employed.  Since  the  implemented  Rotate 
operator  moves  along  the  rotational  axes  in  directions 

v  (E  {+ , 

this  is  the  most  common  —but  not  the  only  —  test  for  rotationally  orthogonal  (or 
tangent)  C-surfaccs.  This  information  is  used  by  the  rotation  experts  to  choose 
rotational  subgoals  that  move  away  from  C-surfaccs. 


Description  of  a  Search  Node 

The  following  information  is  stored  at  each  search  node.  Lazy  evaluation  is 
implemented  so  that  some  of  these  objects  (for  example,  the  set  of  all  applicable 
C-surfaces)  may  not  be  computed  until  they  arc  required. 

(i)  The  configuration  X  of  the  search  node. 

(ii)  The  lattice  point  for  X,  which  is  the-  unique  identifier  for  the  neighborhood 
about  X . 

(iii)  The  applicability  set  at  X . 

(iv)  A,  the  set  of  non-redundant  constraints  at  X ,  sorted  on  increasing  value.  The 
non-redundant  constraints  may  be  approximated  by  the  applicable  constraints 
having  small  positive  (or  zero)  values  at  X. 

(v)  The  parent  node. 

(vi)  The  From- Direction  (The  direction  traversed  from  the  parent  node  to  this 
node). 

(vii)  The  sons  of  this  node.  These  include  “unsuccessful”  explorations  which  did 
not  reach  a  sub-goal,  or  which  reached  a  previously  explored  neighborhood. 

(viii)  The  C-surfaces  on  which  X  lies  which  also  bound  C-Spncc  obstacles,  that  is, 
all  f  G  A  such  that  f{X)  —  0  and  kor(/)  bounds  a  C-Space  obstacle  at  X. 

(ix)  An  Explanation  of  how  this  node  was  reached.  An  explanat  ion  typically  includes 
the  name  of  the  local  expert  that  planned  the  move,  and  enough  information 
to  reconstruct  the  move.  For  example,  the  experts  which  slide  along  level 
C-surfaces  leave  an  explanation  containing  the  names  of  the  constraints,  their 
levels  at  the  parent  node,  and  the  parameterization  chosen  for  the  intersection 
manifold. 

Much  of  the  information  stored  at  a  search  node  is  used  to  record  the  history 
of  the  planning.  An  expert  which  planned  the  move  to  a  search  node  a  will  not  be 
applied  again  with  the  same  parameters.  As  an  example,  consider  the  Intersection 
expert,  which  attempts  to  slide  along  intersection  manifolds,  and  the  Greedy  expert, 
which  attempts  to  move  straight  towards  the  goal.  We  discuss  these  experts  in 
more  detail  in  the  next  section.  If  applied  to  s,  the  C-surface  intersection  export 
will  not  attempt  to  construct  and  slide  along  the  same  intersection  manifold 
which  led  to  s,  unless  it  can  slide  in  a  different  direction  along  the  intersection 
manifold.  Ily  recording  the  From- Direction  for  a  node,  the  planner  can  avoid 
repeating  unfruitful  explorations.  In  particular,  different  experts  can  advise  motion 
in  the  same  direction;  thus  a  particular  intersection  manifold  may  point  in  the 


same  direction  which  was  previously  (or  simultaneously)  attempted  by  the  Greedy 
expert.  Whether  successful  or  not,  recxploratiori  in  this  direction  may  be  avoided  by 
examining  the  From-directions  of  the  sons  of  s.  An  additional  constraint  is  provided 
by  the  F-0111- Direction  of  s  itself:  there  is  typically  no  point  in  exploring  back  in 
the  direction  we  came  from.  The  process  of  leaving  information  for  some  expert 
which  may  be  applied  in  the  future  is  known  as  “forwarding.”  As  we  shall  see,  the 
performance  of  one  expert  can  j  vide  strong  hints  as  to  what  expert  should  be 
applied  next. 

The  planner  computes  local  descriptions  for  the  C-surfaccs  in  A.  Naturally,  parts 
of  these  descriptions  will  change  for  different  subgoals.  The  local  characterizations  of 
C-surfaccs  allow  the  planner  to  find  the  set  of  C-surfaces  to  which  the  goal-direction 
is  tangent  (or  orthogonal)  as  described  above.  When  a  planning  direction  is  chosen, 
these  C-surfaccs  clearly  provide  strong  constraints. 

We  are  now  ready  to  discuss  the  experts  themselves.  The  Bumble  strategy  is 
also  applied  at  each  node,  since  it  is  a  guarantee  of  completeness.  In  light  of  the 
previous  discussion,  we  will  omit  any  discussion  of  the  detection  and  priming  out 
of  explorations  in  unfruitful  directions  (as  determined  by  the  planning  history). 
We  will  consider  the  application  of  particular  experts  to  a  search  node  s  (at 
configuration  X )  which  has  parent  sq. 


2.4.3.  The  Greedy  Expert 


The  greedy  expert  attempts  to  translate  or  rotate  directly  towards  the  goal. 
The  expert  is  necessary  as  an  “end-game’’  strategy,  in  order  to  close  in  on  a 
particular  subgoal  without  worrying  about  finding  the  appropriate  intersection 
manifold.  The  Greedy  expert  illustrates  two  important  heuristics:  forwarding  and 
backing  off.  Suppose  the  greedy  expert  translates  from  a  parent  node  s<j  to  a 
son  ,s.  An  appropriate  explanation  for  the  move  will  be  left  at  5.  If  the  same 
subgoal  is  intact  when  the  planner  explores  s,  the  greedy  expert  will  not  attempt 
translation  again.  Instead,  the  rotation  expert  (see  below)  might  be  invoked.  The 
effect  is  one  of  translating  until  an  obstacle  is  hit,  and  then  rotating  to  get  around 
it.  Alternatively,  the  sliding  expert  (which  slides  along  level  C  surfaces)  might 
be  invoked.  This  coupling  of  experts  is  termed  the  “hit  and  slide”  strategy  (see 
figure  2.4).  However,  the  planner  does  not  directly  rccursc  by  calling  the  sliding 
expert  immediately  after  the  greedy  expert.  Instead,  a  suggestion  is  left,  by  way  of 
explanation  at  5,  and  when  o  is  explored  in  the  search,  the  appropriate  follow-up 
expert  is  invoked.  The  exact  choice  for  which  expert  is  invoked  will  depend  on 
the  history  of  planning  (typically,  what  neighborhoods  and  directions  have  been 
explored  from  sq  and  s),  and  on  the  local  geometry  of  C-suifaces  about  s 


Suppose  that  all  experts  moved  the  robet  as  Far  as  they  could,  that  is,  moved  until 
a  constraint  was  hit  and  left  the  robot  touching  the  constraint.  This  could  result  in 
jamming  the  robot  up  against  many  C-surfae.es  at  once.  It  can  prove  very  difficult 
to  extricate  the  robot  from  this  logjam  situation.  In  fact,  it  is  usually  not  preferable 
to  move  all  the  way  up  to  an  obstacle.  Instead,  we  wish  to  detect  this  intersection 
with  a  planned  trajectory  p,  and  then  back  off  from  the  obstacle  boundary  (along 
p).  Thus  if  p(0)  —  X  and  p(i)  =  Y  is  the  first  intersection  of  p  with  C-Space 
obstacle  boundary,  then  it  makes  good  sense  to  move  to  p(0.8).  This  has  the  effect 
of  leaving  the  robot  in  the  channel  between  obstacles  instead  of  jamming  it  up  in 
corners.  Of  course,  if  it  is  necessary  to  move  to  p(.‘J5)  then  the  greedy  and  Humble 
strategies  will  ultimately  converge. 


<12 


Figure  2.4.  An  idealized  illustration  of  the  hit  and  slide  strategy.  Some  expert  moves  the  robot 
in  direction  i  until  a  C-surl'ace  S  is  hit  at  X.  When  the  planticr  tries  to  move  from  X,  the 
sliding  expert  is  invoked  to  slide  along  S  in  the  goal  direction. 

2.4.4.  The  Intersection  Expert 

The  mathematics  of  intersection  manifolds  in  8?3  X  SO(3)  is  presented  in 
chapters  3  and  4.  The  intersection  expert  attempts  to  find  two  O-surfaccs  in  .4 
■whose  intersection  manifold  contains  a  path  which  makes  progress  towards  a  subgoal. 
The  path  may  be  a  pure  translation  or  a  pure  rotation.  We  will  begin  by  describing 
the  process  of  finding  a  translational  path  which  slides  along  an  intersection 
manifold.  First,  all  C-surfaccs  in  A  which  are  nearly  translationally  tangent  to  the 
goal-direction  arc  selected.  Wc  select  the  first  few  of  these  which  have  the  smallest 
value  at  X.  Ideally,  these  are  the  closest  non-redundant  constraints  at  X.  Call 
this  set  A1.  The  explanations  for  the  moves  from  Sq  to  s  and  from  n  to  any  sons 
of  s  will  yield  a  set  of  previously  explored  intersection  manifolds.  (An  intersection 
manifold  may  be  identified  by  the  name  of  the  intersected  C-surfaces,  their  levels, 
and  the  chosen  parameterization).  The  C-surfaces  in  A'  are  pairwise  intersected 
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(see  chapter  4),  after  appropriate  pruning  as  indicated  by  previously  explored 
intersection  manifolds.  Each  intersection  manifold  (kcr  /)  fj(kcr  g)  is  constructed.  A 
translation  or  rotation  vector  u/iS  is  chosen  such  that  the  path  =  X  +  tv/t9 

slides  along  the  intersection  manifold  of  the  two  level  C-surfaces  ker  /  ami  ker  g 
at  X.  The  intersection  expert  then  selects  the  direction  v /itf  which  is  closest  to 
the  goal  direction  (and  which  is  not  pruned  out  by  consideration  of  the  planning 
history).  Suppose  vj  „  is  a  pure  translation.  The  local  operator  Translate,  is  called 
to  move  from  X  in  direction  vj<g  until  a  C-surface  is  struck'1  or  the  point  on  the 
trajectory  p yifl  which  maximizes  proximity  to  the  goal  is  reached. 

Now,  suppose  Vf  ,j  is  a  pure  rotation.  Our  experimental  implementations  have 
intersected  two  C-surfaces  ker  /  and  ker  g  to  yield  pure  rotational  pa  ths  sliding 
along  the  intersection  manifold  of  ker  /  f]  ker  g  (see  chapter  4  for  the  details).  In 
Proposition  (4.4),  we  demonstrate  that  these  paths  may  be  approximated  to  an 
arbitrary  resolution  by  successive  applications  of  the  local  operators,  with  only  a 
linear  increase  in  the  number  of  path  segments  as  the  resolution  grows  finer.  We 
have  also  found  it  useful  to  approximate  the  rotational  path  along  the  intersection 
as  follows. 

Given  two  level  C-surfaces  ker  /  and  ker  g  at  configuration  X,  we  wish  to 
choose  a  direction  from  X  tangent  to  both.  For  example,  if  the  configuration  space 
were  isomorphic  to  3?'1,  then  kcr  /  and  kcr  g  would  both  be  two  dimensional  surfaces 
in  3-space,  and  this  direction  would  be  V/(X)  X  Vg(X).  (Where  X,  denotes  the 
standard  cross  product  on  SR'5).  In  the  tangent  space  to  a  six-dimensional  C-Space, 
there  are  typically  four  such  tangent  vectors  at  X  which  arc  tangent  to  kcr  /  and 
ker</.  We  will  demonstrate  an  operator  analogous  to  X  which  produces  one  such 
tangent  vector  in  a  natural  way.  (It  is  also  possible  to  solve  for  all  such  tangent 
vectors). 

We  begin  be  defining  ail  extended  product  on  the  tangent  space  to  IR'1  X  SO(  3) 
at  X.  Let  V  =  (14,  V^)  G  T\  be  a  tangent  vector  at  X.  We  may  think  of  Vx  and 
Vy  as  the  translational  and  rotational  components  of  a  six-dimensional  velocity 

'Although  we  also  employ  the  harking  olT  lu-uri'Uic  here. 


vector  V  at  X.  If  W  —  (Wx,  W^)  G  Tx  is  another  tangent  vector  at  X ,  we  define 
the  extended  product  of  V  and  W  by 


V  x  W  =  {Vx  X  Wx,vi}  X  WB). 

The  cross  products  on  the  right  hand  side  arc  simply  the  standard  three-dimensional 
cross  product.  (See  below  (2.3)  for  why  this  makes  sense  for  the  rotational 

A 

components,  Vq  X  W^).  If  V  —  V/  and  W  =  Vg  then  V  X  W  is  tangent  to  both 
ker  /  and  kerg  at  X.  Since  X  only  operates  on  tangent  vectors  to  X  SO(3) 
which  have  the  same  point  of  application,  we  will  never  have  reason  to  confuse  it 
with  X ,  which  can  only  be  applied  to  three-dimensional  tangent  vectors. 

Let  /,  g  G  A'  be  C-functions  generating  the  C-surfaces  ker  /  and  kerg  at  X. 
Observe  that  the  tangent  vector  Vf(X)  X  Vy(X)  is  tangent  to  both  ker  /  and 
ker  y  at  X.  We  can  locally  approximate  a  pure  rotational  trajectory  sliding  along 
the  intersection  of  j  and  y  by  a  path  in  direction 


*g(V/(X))  X  tq(V9(X)).  (2.3) 

Note  that  this  is  well  defined  since 


f(d£  dj_  of  a/  d_i  df\\  =  (df  as  df\ 

x  V  dx  ’  dy  ’  dz  '  dip  ’  30  ’  d<f>  J  J  \d/J>  ’  dO  '  d<j>) 

The  differential  rotations  from  X  are  isomorphic  to  a  three  dimensional  vector 
space,  and  hence  the  cross  product 


dg 


is  also  well  defined,  and  guaranteed  to  be  tangent  to  ker  /  and  kerg  at  X.  The 
Rotate  operator  can  be  called  in  succession  on  the  largest  components  of  (2.3) 
in  order  to  approximate  the  sliding  trajectory.  Of  course,  it  is  also  possible  to 


2.4.5.  The  Sliding  Expert 


The  sliding  expert  attempts  to  find  a  path  sliding  along  one  level  C-surface 
at  X,  which  makes  progress  towards  the  goal.  The  sliding  expert  can  be  thought 
of  as  a  less  constrained  version  of  the  intersection  expert.  The  sliding  expert  tries 
to  choose  a  C-surfacc  in  Af  to  which  the  goal-direction  is  (almost)  tangent.  As  we 
will  see  in  chapter  4,  it  is  possible  to  choose  a  parameterization  along  a  C-surface 
which  maximizes  progress.  This  path  along  the  C-surface  can  then  be  realized  (at  a 
desired  resolution)  by  successive  applications  of  the  local  operators.  However  since 
there  are  many  paths  from  X  sliding  along  a  C-surface  at  X,  we  need  to  develop 
a  good  heuristic  strategy. 

Our  motivation  is  as  follows.  There  arc  an  uncountable  number  of  paths  from 
X  sliding  along  a  C-surface  at  X.  We  could  maximize  a  directional  derivative  at 
X  to  choose  ?  locally  optimal  search  direction.  This  would  work  once;  however, 
this  would  not  solve  the  problem  of  state:  it  is  necessary  to  partition  the  set  of 
paths  into  “neighborhoods,”  and  to  mark  a  neighborhood  of  paths  as  explored 
when  a  representative  from  that  neighborhood  is  selected  and  attempted  by  a  local 
operator.  In  principle,  a  computation  involving  homotopic  equivalence  classes  is 
possible  (see  Donald  ( 1933a)  and  appendix  III).  However,  this  requires  a  global 
computation  in  C-Space.  In  particular,  the  image  of  all  paths  in  an  equivalence 
class  may  cover  X  50(3),  even  if  there  are  several  classes.  We  wish  to  find  a 
way  to  partition  the  paths  from  X  into  neighborhoods,  sample  a  canonical  element 
from  the  neighborhood,  and  evaluate  it  as  a  local  move  in  the  search. 

Given  a  C-surface  normal  V/  at  X ,  wc  wish  to  choose  a  direction  v  sliding  along 
the  C-surface  ker  /  which  maximizes  progress  to  a  subgoal.  Let  B  (x  ,y,z,ip,d,(f>) 
be  the  obvious  orthonormal  basis  for  the  tangent  space  to  3J3  X  50(3),  and 
-B  =  (-x,  ~y,  -z,  -1/q  -0,  -<£). 

Next,  wc  form  a  set  of  vectors  orthogonal  to  V /( X)  as  follows: 

{V/(X)}<g)(sU-*) 
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where  P  <S>  Q  =  {p  X  <7  |  p  fc  P,  <?  £  i2  }•  AH  of  these  vectors  arc  orthogonal  to 
ker /  at  X.  We  then  choose  the  direction  6  £  Z>  which  maximizes  —  A")), 

where  the  C  —  X  is  the  goal  direction.  If  4> x  is  the  heuristic  product  on  tangent 
vectors  inscad  of  the  single-valued  RicmaMiian  inner  product,  then  both  components 
of  the  image  of  should  be  maximized.  In  chapter  4  ,  we  will  see  that  it  is 
possible  to  comply  as  closely  as  desired  to  the  C-surface  ker  /  while  traveling  in 
direction  v. 

To  understand  this  strategy,  consider  the  following  example:  Suppose  we 
employ  a  basis  B'  which  only  spans  Then  the  expert  will  choose  the  available 
translation  sliding  along  the  level  C-surface  which  maximizes  progress  towards  'he 
goal.  Once  the  direction  v  is  chosen,  the  Translate  operator  is  invoked  to  slide 
along  the  level  C-surface  until  a  constraint  is  reached. 

There  is  no  need  for  the  basis  B  to  be  orthogonal;  this  was  merely  adopted  for 
the  sake  of  intuitive  development.  The  basis  provides  a  sampling  of  the  function 
space  of  paths  compliant  to  the  C-surface  about  X. 

A  Conjecture  on  Completeness  using  Extended  Spanning  Sets 

By  using  the  basis  B,  we  obtain  a  12-way  sampling  of  the  space  of  directions 
orthogonal  to  V/  at  X — in  other  words,  there  are  12  vectors  in  D.  Imagine  using 
another  set  of  vectors,  3  1  ,  which  is  larger  than  B,  to  construct  D.  Then  D  would 
provide  a  finer  sample  of  the  space  of  directions,  since  more  directions  would  be 
sampled.  In  principle  it  should  be  possible  for  a  sample  to  be  complete  at  a  given 
resolution.  We  formalize  this  idea  as  follows: 

A  spanning  set  for  a  space  V  is  a  set  of  vectors  which  spans  V  yet  which  is 
not  necessarily  a  basis.  A  spanning  set  is  a  basis  for  V  which  has  been  extended 
by  adding  other  vectors.  We  conjecture  that  there  exist  certain  spanning  sets 
which  might  be  employed  to  construct  a  complete  planning  algorithm  without  the 
Bumble  strategy.  What  constitutes  such  a  complete  spanning  set?  The  analogue  of 
rcsolutic  i  for  an  arbitrary  spanning  set  B *  would  consist  in  (l)  the  cardinality  of 
the  spanning  set  and  (2)  the  uniformity  of  distribution  of  the  vectors 
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about  the  unit  five-dimcusional  sphere  S’’  in  the  tangent  space  at  X.  The  greater 
the  number  of  vectors  in  the  spanning  set,  and  the  more  uniform  their  distribution 
about  S5,  the  finer  the  resolution  of  the  planner.  The  development  of  such  a  planning 
algorithm  requires  surmounting  additional  theoretical  and  technical  difficulties. 
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Figure  2.5-  An  idealised  illustration  or  tlie  hit  and  rotate  strategy.  Some  expert  moves  the  robot 
in  direction  v  until  a  C-surface  5  is  hit  at  configuration  X.  When  the  planner  tries  to  plan  a 
move  from  X,  the  rotation  expert  is  called  to  calculate  a  rotation  away  from  S  (in  direction  <f>). 
From  the  new  configuration,  direction  v  can  be  pursued  again. 

2.4.6.  The  Rotation  Expert 

The  rotation  expert  is  built  on  the  rotational  operator  Rotate,  and  is  designed 
to  handle  some  of  the  special  problems  of  moving  through  rotation  space  that  are 
discussed  in  chapters  3,  4,  and  5.  The  rotation  expert  might  be  called  to  accomplish 
a  simple  rotational  subgoal,  or  in  conjunction  with  some  more  elaborate  strategy. 
In  particular,  when  a  translational  motion  terminates  by  striking  a  C-surface, 
forwarding  messages  are  left  for  both  the  sliding  expert  and  the  rotation  expert. 
The  former  has  been  discussed  as  the  “hit  and  slide”  strategy  (figure  2.4);  the  latter 
is  known  as  the  “hit  and  rotate”  technique  (figure  2.5). 

The  first  problem  that  the  rotation  expert  must  deal  with  is  the  “wrap  around” 

A  A 

in  rotation  space.  A  subgoal  <j>u  can  be  reached  in  directions  and  —  <f>,  although 
typically  one  is  “shorter”.  In  conjunction  with  the  planning  history,  the  rotation 
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expert,  on  successive  applications  to  the  same  node,  can  develop  strategies  for 
rocking  back  and  forth  on  a  slice  of  rotation  space. 

The  Rotate  operator  is  more  constrained  than  the  Translate  operator  (in  that 

AAA 

it  can  only  be  applied  in  ±ip,  ±  ),  and  ±<j>).  lienee  the  rotation  expert  must  have 
a  method  for  approximating  rotational  trajectories  (specified  in  angle  space)  which 
arc  linear  combinations  of  the  rotational  basis  vectors,  such  as 

v  —  aip  4-  bO  -f  c<f>  (2-4) 

for  some  scalars  a  ,  b,  and  c. 

In  terms  of  the  completeness  of  the  algorithm,  there  is  no  need  for  a  rotate 
operator  in  direction  (2.4)  (provided  a  path  along  v  lies  in  open  sets  of  free  space). 
In  chapter  4,  we  show  that  a  continuous  path  may  be  approximated  as  closely  as 
desired  by  a  sequence  of  moves  along  the  rotational  axes,  and  that  the  number 
of  staggered  path  segments  required  grows  only  iineariy  as  the  resolution  becomes 
finer.  In  practice  this  use  of  the  restricted  rotate  operator  has  proved  adequate 
in  our  path-finding  experiments.  However,  it  is  heuristically  useful  to  realize  such 
paths  as  accurately  as  desired,  since  this  allows  higher  level  experts  to  suggest 
arbitrary  rotational  trajectories.  Given  such  a  trajectory,  the  rotational  directions 
are  ranked  by  magnitude  of  change,  and  the  unexplored  direction  of  greatest  change 
is  first  attempted.  On  failure,  or  upon  successive  applications  of  the  rotation  expert 
to  the  search  node,  the  other  directions  in  (2.4)  will  be  attempted.  This  process 
leads  to  the  approximation  of  arbitrary  pure  rotations  by  a  staggered  sequence  of 
rotations  along  the  axes.  If  the  extent  of  each  rotation  is  limited,  the  approximation 
can  be  made  arbitrarily  line.  To  approximate  motion  in  a  direction  such  as  (2.4),  the 
planner  actually  attempts  several  or  the  directions  simultaneously,  which  results  in 
a  spanning  “box”  of  rotational  moves  about  the  idealized  trajectory  (in  the  absence 
of  obstacles). 

Suppose  a,  b,  and  c  in  the  idealized  trajectory  (2.4)  are  positive.  This  yields 
a  set  of  positive,  or  "forward"  rotational  directions,  and  a  set  of  “backwards” 
rotational  directions  which  can  attain  the  goal.  Which  directions  arc  forward  and 


which  are  backward  depend  upon  the  distance  (in  the  vector  parameter  space  V ) 
of  the  goal  from  X,  that  is,  on  n&(G  —  X).  For  example,  if  G^-X^  is  negative  and 

A  A 

small,  then  -f  ^  will  be  a  backwards  direction,  and  —  4>  will  be  a  forward  direction. 

The  rotation  expert  develops  and  ranks  these  sets  of  forward  and  backward 
rotational  directions.  I3y  examining  the  planning  history  and  the  local  geometry  of 
C-surfaccs  at  X ,  these  sets  of  directions  are  ;  *urn  pruned.  In  particular,  local 
C-surfaccs  that  would  block  a  particular  rotational  motion  arc  detected.  For  a 
direction  v,  this  is  done  by  examining  the  magnitude  of  the  directional  derivative 
in  v.  The  importance  of  such  an  impediment  is  then  heuristically  ranked  by  the 
closeness  of  the  C-surfacc  at  X .  Special  consideration  is  given  to  C-surfaccs  which 
have  a  history  of  proving  troublesome.  For  example,  when  an  expert  runs  into  a 
C-surface,  the  reason  for  stopping  is  left  as  part  of  the  move  explanation.  If  the 
rotation  expert  is  invoked  as  part  of  a  ‘‘hit  and  rotate”  strategy,  then  we  must 
ensure  that  the  planner  tries  to  rotate  away  from  the  C-surface(s)  which  blocked 
progress.  The  rotational  directions  which  point  away  from  C-surfaccs  may  be  found 
by  examining  V /.  The  process  of  determining  the  rotational  constraints  from  the 
local  geometry  of  C-surfaces  is  closely  related  to  our  earlier  discussion  of  detecting 
rotationally  orthogonal  C-surfaccs. 

Thus  the  requested  rotational  trajectory  and  rotational  goal  provide  a  sot 
of  desired  rotational  motions.  The  planning  history  supplies  a  set  of  rotational 
constraints,  and  from  the  local  C-surfacc  geometry  can  be  inferred  a  set  of 
preferred  and  prohibited  motions.  The  constraints,  preferences,  and  prohibitions 
arc  intersected  with  the  forward  and  backward  desires.  This  yicids  a  set  of  rotational 
directions  which  will  be  attempted  using  the  Rotate  operator.  Depending  on  the 
kind  of  invocation,  the  rotation  expert  may  apply  the  Rotate  operator  up  to 
some  fixed  number  of  times  -this  is  particularly  useful  when  it  must  attempt  to 
approximate  an  idealized  rotational  trajectory  which  is  a  linear  combination  of  the 
basic  rotational  directions. 

Canny  (lfiS-l)  has  recently  extended  the  Rotate  operator  for  directions  such  as 
cq.  (2.'l),  corresponding  to  uniform  rotation. 


b'puro  2.G.  An  idealiied  illustration  of  the  arouild  expert.  When  progress  for  the  moving  object 
in  the  goal  direction  v  is  blocked,  the  expert  attempts  to  find  a  C-smTace  wbicli  is  roughly 
orthogonal  to  v.  A  sliding  motion  (either  v  or  —  v)  is  then  planned  along  this  level  C-surfare 
(around  the  obstacle).  The  resulting  search  node  is  then  expanded. 

2.4.7.  The  Around  Expert 

The  around  expert,  attempts  to  circumnavigate  obstacles  by  sliding  around 
their  boundary.  An  idealized  illustration  of  the  around  expert  is  shown  in  ligure  2.6. 
The  around  expert  is  similar  to  the  sliding  expert,  except  that  instead  of  attempting 
to  find  a  C-surface  which  contains  a  path  towards  the  goal,  the  around  expert 
searches  for  a  C-surface  which  is  (roughly)  locally  orthogonal  to  the  goal  direction. 
Next  a  path  is  planned  sliding  along  this  surface  in  a  direction  v'  orthogonal  to  the 
goal  direction;  the  path  is  attempted  using  a  local  operator.  Typically,  this  motion 
will  result  in  a  search  node  s1  which  is  farther  from  the  goal  than  the  parent  node, 
s.  Ordinarily,  s'  would  not  be  explored  soon,  since  other  search  nodes  would  appear 
more  promising  to  the  planner’s  best-first  strategy.  In  order  to  b'lve  the  around 
strategy  a  chance,  the  around  expert  explicitly  places  s'  at  the  front  of  the  search 
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queue  and  calls  the  planner  recursively. 


The  around  expert  can  also  invoke  the  intersection  expert.  Recall  that  the 
intersection  expert  normally  tries  to  construct  tangent  intersection  manifolds  which 
contain  paths  towards  the  goal.  However,  when  called  from  the  around  strategy, 
it  can  construct  intersection  manifolds  locally  orthogonal  to  the  goal  direction.  To 
construct  the  intersection  set  of  locally  orthogonal  level  C-manifolds,  we  perform  a 
pairwise  intersection  of  C-manifolds  locally  orthogonal  to  the  goal  direction  at  X . 


2.4.8.  The  Suggestor 

The  suggestor  is  a  strategy  for  proposing  good  subgoals  in  configuration  space. 
As  we  saw  in  Donald  (1983a),  one  of  the  problems  with  local  operators  even  if 
they  arc  complete  (that  is,  their  closure  covers  configuration  space),  is  that  without 
good  subgoals,  they  may  take  a  long  time  to  converge.  The  suggestor  is  a  heuristic 
strategy  for  setting  subgoals  in  C-Space. 
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searched  for  a  sequence  Q  of  free  configurations  ( not  a  path)  stepping  through  the 
lattice  to  the  goal.  If  no  such  sequence  can  be  found,  then  configurations  on  a 
promising  partial  sequence  arc  employed.  These  configurations  may  then  he  set  as 
subgoals,  and  the  planner  can  be  called  recursively.  The  configurations  Q  represent 
intermediate  planning  islands  of  safe  configurations.  If  paths  can  be  found  between 
these  configurations,  then  the  find-path  problem  is  solved.  Otherwise,  expanding 
from  any  partial  piths  found  can  also  prove  useful,  in  that  the  planning  islands 
effectively  distribute  the  application  of  local  experts  and  operators  over  more  of 
configuration  space. 


The  suggestor  complicates  the  connectivity  of  the  explored  neighborhoods 
graph.  The  ability  to  explore  arbitrary  subgoals  and  suggested  paths  requires 
more  complicated  bookkeeping  for  neighborhood  exploration:  we  must  employ  the 
connect  strategy ,  in  order  to  know  when  partial  paths  link  up.  If  partial  paths 
not  rooted  at  the  start  neighborhood  are  permitted,  then  the  graph  of  explored 
neighborhoods  will  not  necessarily  be  connected,  and  the  mark  strategy  will  fail  (the 
mark  strategy  constructs  a  directed,  spanning  tree  for  a  connected,  rooted  graph 
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Listing  Is  Tlic  log  of  expert  explanations  for  the  path  in  figure  2,7. 


(find-path  *sl  *gl) 

Verifying  the  start  and  goaf  points... 

start  :  (00011  11),  goal  :  (-6  10  0  0  0  0) . 

Starting  soarch,  bees... 

Exploring  (0001  1  11)... 

Local  Expert:  I  translated  straight  towards  goal,  reaching  ( (— 1  10  11  11)) 
Exploring  (-1  1  0  1  1  il) . .  . 

Local  Expert:  I  Slid  along  a  level  C-Manifold,  reaching  ((-6  1  C  1  1  11)) 
Exploring  (-6  1011  11)... 

Local  Expert:  I  translated  straight  towards  goal,  reaching  ((-6  10  0  1  1  11)) 
Exploring  (-6  10  0  1  1  11)... 

Rotation-Export:  Found  0  guiding  constraints  on  rotational  motion. 
Rotation-Export:  Intoroected  Rotational  Constraints  with  desired 
rotations  yielding  possiblo  motions  in 
((MINUS  PHI)  (MINUS  PSI)  THETA). 

Rotation-Export:  I  am  trying  to  rotato  in  (PLUS  THETA)  ... 

Local  Export:  I  rotated  to  reach  ((-6  10  0  1  1  0)) 


Exploring  (-8  10  0  1  1  0) . . . 

Rotation-Export:  Found  0  guiding  constraints  or.  rotational  motion. 

Rotation-Expert :  Intoriiect.ed  Rotavional  Constraints  with  desired 
rotations  yielding  possible  motions  in 
((MINUS  PHI)  (MINUS  PSI)) . 

Rotation-Export:  I  am  trying  to  rotate  in  (MINUS  PHI)  ... 

Local  Export:  I  rotated  to  reach  ((-6  10  0  0  1  0)) 

Exploring  (-8  10  0  0  1  0) . . . 

Rotation-Export:  Found  0  guiding  constraints  on  rotational  motion. 

Rotation-Export:  Intersected  Rotational  Constraints  with  desired 
rotations  yielding  possible  motions  in 
((MINUS  PSI)) . 

Rotation-Export:  I  am  trying  to  rotate  in  (MINUS  PSI)  , . . 

Local  Expert :  I  rotated  to  reach  ((-6  10  0  0  0  0)) 

Exploring  (-6  iO  0  0  0  0) . . . 

[i'uccobb!]  Saving  and  Drawing  final  path... 

Back  to  Lisp  Top  Level  in  Lisp  Listener  2 
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Figure  212.  View  II:  (frame  19),  The  final  configuration. 

2.5.  Examples  of  the  Local  Experts  In  Use 

In  figure  2.7,  we  show  a  very  simple  example  of  a  path  found  using  local 
experts.  Listing  I  shows  a  log  of  the  expert  explanations  for  each  move. 

The  “Thor’s  Hammer”  example  in  chapter  1  was  produced  by  disabling  all 
experts,  and  employing  only  the  Bumble  strategy.  (Please  refer  to  this  figure).  In 
the  accompanying  figures  (2.8-13),  we  chow  a  path  found  by  a  strategy  comprising 
all  the  experts  described  above.  The  solution  path  is  very  different,  and  tends  to 
slide  around  obstacles  instead  of  finding  convoluted  paths  between  them. 

Figures  2.14-21  show  the  solution  for  a  find-path  problem  in  a  cartesian 
workspace.  A  cartesian  workspace  is  a  bounding  box  beyond  which  the  reference 
point  may  not  translate.  However,  the  bounding  box  imposes  no  restrictions  on 
rotations.  The  Movers’  problem  in  a  cartesian  workspace  is  similar  to  the  motion- 
planning  problem  for  cartesian  manipulators,  and  the  L-shapcd  object  may  be 
thought  of  as  the  (wrist  and)  payload.  First,  we  show  the  reference  point  on 
the  L-shaped  object.  Next  two  views  are  presented  of  the  path  found  within  the 
workspace,  around  a  large,  diagonally-placed  obstacle.  View  (II)  is  a  view  from 
the  side;  view  (I)  is  a  view  from  the  top.  Only  the  back  faces  of  the  rcctangloid 
workspace  are  shown.  Since  the  rotation  from  frames  13  to  14  is  very  large  (>  7r 
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Figure  2.13.  View  III:  {A  detail  of  frames  1  through  6). 


in  the  — V>  direction),  a  detail  of  the  rotation  is  also  shown. 


2.6.  Path  Planning  versus  Discrete  Intersection  Detection 

Imagine  a  brute-force  planner  which  disaetizes  configuration  space,  places  the 
robot  at  every  point  in  the  discretization,  and  tests  for  intersection.  This  would 
yield  a  discrete  set  of  configurations  where  the  robot  could  be  placed.  Alternatively, 
the  tests  could  be  structured  in  a  search.  As  stated  so  far,  this  is  not  collision-free 
path  planning.  Path  planning  ensures  that  a  path  exists  between  each  configuration 
on  the  path.  It  has  been  argued  that  if  the  intersection-detection  is  done  at  a 
fine  enough  resolution  then  a  path  will  have  been  effectively  found.  At  a  given 
resolution,  it  is  possible  to  bound  the  size  of  the  intersection  between  the  robot 
and  any  obstacle  which  can  occur  between  intersection  checks.  This  bound  grows 
smaller  as  the  sampling  of  the  space  grows  finer.  By  growing  the  real-space  obstacles 
by  this  bound,  it  is  possible  to  ensure  that  no  collisions  occur  between  discrete 


E I.  Problem  7) 


till 


Figure  2.17.  Solution  Path,  View  (11),  frames  (19  21) 


Figure  2.21.  Detail  of  the  rotation  from  frarnca  13-14. 

intersection  checks  (Gouzcncs  (1983)).  Of  course,  if  the  resolution  is  insufficiently 
fine,  then  the  obstacles  may  be  grown  so  much  that  no  path  can  be  found. 

For  gross  motion  planning  in  an  uncluttered  environment,  this  approximate 
method  may  perform  reasonably  well.  In  complicated  environments,  however,  the 
resolution  will  have  to  be  fine  in  order  to  ensure  that  paths  are  collision  free 
without  growing  the  obstacles  so  much  that  no  path  can  be  found.  We  will 
compare  the  asymptotic  complexity  of  the  discrete  intersection  method  with  the 
Rotate  operator.  (The  Rotate  operator  is  the  most  complex  local  operator).  The 
fundamental  observation  is  that  the  complexity  of  the  discrete  intersection  method 
varies  linearly  with  the  sampling  resolution,  whereas  the  complexity  of  the  Rotate 
operator  is  independent  of  (any)  resolution.  This  is  because  our  discretization  is 
quite  different:  a  lattice  is  thrown  on  the  space  in  order  to  record  the  state  of  the 
planner  and  the  connectivity  of  the  explored  neighborhoods. 


Consider  the  following.  Suppose  X  and  Y  are  configurations  on  a  rotational 
trajectory  in  direction  Suppose  further  that-  the  robot  is  composed  of  m  convex 
polyhedia  containing  k  generators  each,  and  that  there  are  n  convex  obstacles 
containing  j  generators  each.  The  number  of  faces  on  a  robot  polyhedron  or  an 
obstacle  polyhedron  is  0(k )  (respectively,  O(j)).  To  perform  one  intersection  check 
(at  a  single  configuration)  for  one  robot  polyhedron  and  one  obstacle  polyhedron 
requires  time  0(\og*(j  +  k))  (Dobkin  arid  Kirkpatrick  (1980)).  This  theoretical 
intersection  algorithm  has  not  yet  been  implemented,  but  we  consider  it  since  it 
is  the  fastest  known.  To  perform  a  check  (at  one  configuration)  for  the  entire 
robot  against  the  entire  obstacle  environment  requires  time  0(mn  log2(j  +  fc)).  Now 
suppose  that  the  path  segment  [X ,Y j  must  be  sampled  i  times  foi  the  quantizing 
intersection  checker.  This  requires  time 


0[imn  log2(_;  +  k )). 

In  chapter  b,  wc  show  that  our  planner’s  Roiute  operator  could  determine  whether 
there  exists  a  path  from  X  to  Y  in  time  Q(/VlogN)  (where  N  is  the  number  of 
O-surfaces).  In  chapter  3,  we  show  that  N  —  0(mnjk).  Hence  the  complexity  for 
Rotate  is 


0{mnjk  \og(mnjk))  0{mnj k(\ogrn  +  log7i  +  logj  +  log  k)). 

Rotate  ensures  that  there  exists  a  path  from  X  to  Y  without  growing  the  real-space 
obstacles,  and  does  not  involve  a  resolution  factor  i.  Holding  k  and  j  fixed,  the 
relative  asymptotic  performance  of  the  quantizing  intersection  def  ;ctor  and  the 
Rotate  operator  will  depend  on  whether  or  not 


i  >  log(mn). 

The  constants  i,  j,  and  k  will  depend  on  particular  workspaces  and  find-path 
problems.  However,  wc  believe  that  in  order  to  be  reasonably  sure  of  the  safeness 
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of  a  path  between  configurations  without  growing  the  real-space  obstacles  too 
much,  i  may  have  to  be  quite  large.  This  is  especially  true  in  reasonable  sized 
environments.  So  as  mn  increases,  the  workspace  becomes  more  crowded  and/or  the 
robot  becomes  more  complicated,  and  the  sampling  rate  will  have  to  be  increased. 
We  think  it  unlikely  that  the  sampling  rate  will  grow  only  logarithmically  with 
the  workspace  complexity.  Moreover,  the  theoretical  0(log2(j  4-  k ))  intersection 
time  for  the  Dobkin  and  Kirkpatrick  (1980)  algorithm  assumes  that  the  solid 
models  of  the  m  robot  polyhedra  are  precomputed.  (If  the  solid  models  must  be 
computed  for  each  configuration,  then  this  will  take  0{km )  additional  time  per 
sample  point).  In  addition,  for  a  polyhedron  with  k  faces,  0(k\ogk )  preprocessing 
time  is  required  by  the  algorithm  (for  each  intersection  check),  which  would 
yield  an  even  higher  complexity  for  the  discrete  path  planning  algorithm.  At  this 
stage,  since  the  algorithm  Is  unimplcmented,  it  is  unclear  whether  some  sort  of 
lazy  evaluation,  parametric  representation,  or  efficient,  precomputation  could  be 
employed  to  reduce  the  complexity  of  iterative  application  of  this  intersection  test. 
Most  implemented  intersection  detectors  that  are  reasonably  robust  have  time 
complexity  0((j  +  k)  log(j  +  k))  or  0((j  4-  A:)2).  However,  it  is  possible  to  employ 
minimum  distance  checks,  or  0{j  +  k )  intersection  checks  in  some  cases. 

Summary 

In  a  practical  planning  system,  there  are,  of  course,  other  considerations.  For 
example,  our  employment  of  the  Rotate  operator  requires  time  to  update  the  lattice. 
The  main  point  is  as  follows:  on  a  lattice  of  spacing  d,  to  verify  the  safeness  of  a 
path  of  length  di,  the  discrete  intersection  method  requires  at  least  time  0{imn), 
whereas  the  Rotate  operator  requires  time  0{mn  log(?rm)).  The  discrete  method 
actually  does  not  ensure  safeness,  but  merely  that  the  intersection  “size”  is  no 
greater  than  some  function  of  d. 

Competence  versus  Performance 

We  have  shown  that  the  relative  performance  of  the  two  algorithms  will  largely 
depend  on  the  constants  in  the  problem.  For  gross  motion  in  uncluttered  workspaces 
the  discrete  intersection  algorithm  will  probably  perform  better.  In  complicated, 
crowded  environments,  or  in  problems  requiring  motions  close  to  the  obstacles,  the 
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required  sampling  rate  will  probably  be  prohibitive.  In  addition  to  the  question 
of  performance,  we  should  also  mention  the  issue  of  competence.  (In  linguistics, 
competence  refers  to  the  knowledge  base,  and  performance  refers  to  how  well  it  is 
used).  The  representations  we  develop  in  subsequent  chapters  are  applicable  not 
only  to  the  find-path  problem  with  six  degrees  of  freedom,  but  also  to  the  class  of 
geometric  planning  problems  described  in  chapter  1  (for  example,  fine  motion,  and 
planning  with  uncertainty).  It  is  clear  from  previous  work  that  these  problems  are 
within  the  competence  of  the  representation  we  develop  for  SR3  X  50(3)  (Mason 
(1981),  Lozano-Peren,  Mason,  and  Taylor  (1983),  Erdmann  (1984)).  At  this  point  we 
have  no  indication  that  these  problems  are  within  the  competence  of  the  discrete 
intersection  method.  (Find-space,  however,  can  be  accomplished  using  discrete 
intersections). 
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Questions  of  Representation:  C-functions  and  Applicability 
Constraints  in  a  Six  Dimensional  Configuration  Space 

In  this  chapter,  wc  lirst  present  a  formal  framework  in  which  several  open 
Questions  about  configuration  space  constraints  may  be  resolved*  Tbis  framework 
has  been  discussed  informally  in  the  first  two  chapters.  We  then  proceed  to  construct 
and  prove  a  set  of  theorems  about  the  domains  and  domain  topology  of  C-functions 
for  the  classical  Movers’  problem  with  six  degrees  of  freedom. 

These  theorems  allow  us  to  define  the  applicability  constraints  on  C-functions  for 
the  Movers’  problem  in  X  50(3).  Every  C-function  characterizes  a  constraint  on 
motion  only  within  a  certain  region  of  rotation  space.  Determining  what  constraints 
are  applicable  at  a  given  orientation  (or  range  of  orientations)  is  of  fundamental 
importance  to  the  mathematical  framework  for  the  spatial  planning  problem:  in 
order  to  plan  using  constraints,  we.  must  know  where  (at  what  orientations )  these 
constraints  are  applicable.  Recall  that  each  C-function  is  generated  by  a,  pair  of 
boundary  cells  (a,  b),  where  a  lies  on  the  boundary  of  a  moving  polyhedron  and  6  on 
the  boundary  of  an  obstacle  polyhedron.  Put  simply,  the  applicability  constraints 
determine  what  boundary  cells  a  and  b  can  interact  at  a  given  orientation. 

3.1.  Definitions  and  Conventions 

Let  A  denote  any  rigid,  convex  set.  A(Q)  denotes  A  rotated  to  orientation  0. 
Formally,  if  ©  is  an  orientation,  and  £(0)  is' the  corresponding  rotation  operator, 
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then  /1(0)  denotes  ,5(6)  applied  to  A.  As  a  kind  of  shorthand,  we  refer  to  A(0)  as  “A 
at  orientation  0,”  or  “A  rotated  to  orientation  ©.”  For  example,  if  F  is  a  face,  then 
F(0)  denotes  F  at  orientation  ©.  F’s  normal,  N,  rotates  with  F,  and  is  denoted 
N(Q).  We  assume  face  normals  arc  outward-directed  from  the  polyhedra  they 
bound.  We  will  in  general  use  A  to  denote  a  corivex  moving  polyhedron,  and  I)  for  a 
convex  obstacle  polyhedron.  If  ea  is  an  edge  of  A  and  mid(ea)  denotes  its  midpoint, 
then  mid(ea(©))  denotes  its  midpoint  at  orientation  0.  At  this  point  it  is  not 
convenient  to  commit  ourselves  to  any  particular  representation  for  3-dirnensiona! 
rotations.  However,  the  reader  may  without  essential  loss  of  generality  interpret 
o(0)  (for  v  €  3J{)  as  the  rotation  matrix  >5(0)  applied  to  the  vector  v,  where  >5(0) 
might  be  parameterized  by  Euler  Angles.  Since  >5(0)  is  an  orthonormal  matrix, 
(>5(0)]-1  =  [>5(©)]T  can  be  employed  to  rotate  a  plane  which  is  represented  as  a 
4-drmensional  vector.  This  operation  yields  the  rotated  normal  A^(0)  of  course  (see 
Paul  (1981)).  However,  note  that  the  results  of  this  chapter  arc  independent  of  any 
particular  representation  of  rotations,  and  that  >5(0)  is  properly  a  generic  rotation 
operator,  u  •  v  denotes  the  standard  inner  product  on  !Ji'!  of  u  and  v.  If  u  and  v  are 
complicated  expressions,  however,  wc  will  use  the  notation  (u,u). 

The  six  dimensional  configuration  space  51?'*  X  50(3)  is  formally  defined  in 
chapter  2.  X  will  denote  a  configuration  in  this  space.  We  will  identify  0  with 
>5(0)  and  write  ©  E  50(3).  Writing  X  —  ( x ,  0)  makes  explicit  the  translational 
component  of  the  configuration  (x)  and  the  rotational  component  (0  or  >5(0)). 

d  denotes  the  boundary  operator.  For  example,  if  F  is  a  face  on  a  polyhc  iron 
B,  then  dF  denotes  the  ring  of  edges  which  bound  F.  dB  denotes  the  faces  of  B, 
dc  for  an  edge  c  denotes  e'u  vertices,  and  so  forth.  The  coboundary  operator  is  the 
dual  of  the  boundary  operator  and  is  denoted  6.  The  coboundary  of  a  vertex  is  the 
set  of  edges  incident  there;  the  coboundury  of  an  edge  arc  the  faces  which  the  edge 
bounds;  and  the  coboundary  of  a  face  is  the  zero,  one,  or  two  solids  it  bounds.  In 
chapter  5,  v/c  provide  a  forrnai  definition  of  boundary  and  coboundary  using  the 
chain  groups;  alternatively,  see  Hockirg  and  Young  (lOfiij  or  Gibliu  (1.977). 

We  denote  the  faces,  edges,  and  vertices  of  a  polyhedron  B  by  j'ace.s{U), 
c dges(B),  and  vcri(B),  respectively. 
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If  5  is  a  set  then  i[S)  denotes  its  interior,  and  kS  its  closure.  teS  =  r(S)[JdS. 

We  denote  the  classical  Movers’  problem  with  six  degrees  of  freedom  by  GDOF. 

3.2.  Representing  Constraints  in  Configuration  Space 

Lozano-Pcrez  (1983)  showed  that  the  C-Space  obstacles  can  be  represented  as 
an  intersection  of  a  finite  number  of  half- hyperspaces,1  where  each  half- hyperspace 
is  represented  via  a  constraint  function  of  the  form 

/,  :  X  50(3)  -  3? 

where  the  sign  of  fi(X)  determines  whether  X  is  inside,  on,  or  outside  the  C-Space 
obstacles.  However,  when  rotations  are  allowed,  each  constraint  function  is  valid, 
or  applicable  only  within  a  certain  region  Ai  of  the  rotation  space: 

fi  :  SR3  X  Ai  ->  3?  (Ai  C  50(3)). 

We  call  such  a  function  /,  a  C-function.  We  consider  the  robot  and  obstacles  to 
be  modeled  by  the  (possibly  overlapping)  union  of  convex  polyhedra,  and  define 
a  boundary  cell  to  be  a  face,  edge,  or  vertex  of  such  a  polyhedron.  C-funclions 
model  constraints  on  motion  generated  by  pairs  of  cells  (g„,  gi ,)  where  ga  and  g j,  are 
boundary  cells  on  the  robot  and  on  an  obstacle,  respectively.  Lozano-Pcrez  (1983) 
identified  three  types  of  interactions:  (face, vertex),  (vertex, face),  and  (edge, edge), 
which  to  preserve  tradition  we  shall  term  type  (a),  (b),  and  (c)  constraints.  However, 
these  interactions  can  only  occur  in  certain  orientations;  for  example,  it  is  easily  seen 
that  although  two  cuboids  generate  I'M  type  (c)  constraints,  at  any  fixed  orientation 
only  certain  edges  can  interact  and  hence  only  certain  type  (c)  constraints  arc 
applicable,  The  region  o-  rotation  space  where  a  C-function  /,•  is  applicable  is  it’s 
applicability  region,  ,fi,.  The  domain  of  /,-,  then,  is  X  Ai. 

For  the  two-dimensional  Movers’  problem,  the  rotation  space  is  the  1-sphcre 
and  the  applicability  legions  At  arc  simply  sectors  on  S1.  While  Lozano-Perez 
1  roi  a  moving  olijial  ami  obsl act. h  repieaenled  as  overlapping  unions  of  eonvex  polyliedra. 


I '25 


i  \ja  rca  ni  -ut  o.* 


(1083)  was  able  to  dcDiic  the  form  of  C-Space  constraints  /,  for  G DOF,  previous 
work  has  not  been  able  to  formulate  the  applicability  regions  in  50(3). 

We  begin  by  defining  CO  C  5JJ-1  X  50(3),  the  space  of  forbidden  configurations: 


CO  =  {X\\/Ca(X)} 


(1) 


where  Cct  is  a  constraint  sentence  (see  Brooks  and  Lozano-Perez  (1983)).  a  is  indexed 
by  C- Space  obstacles.  For  each  C -Space  obstacle  Oa,  Ca  maps  a  configuration  X 
to  true  or  false,  depending  on  whether  X  is  inside  0<j ■  (l)  states  that  if  X  is  inside 
any  C-Space  obstacle,  then  it  is  in  CO. 

For  X  —  (i,  ©), 


ca{x,  0)  =  a(©  e  A-  =t  /,(*,  0)  <  o). 


(2) 


Let  us  parse  (2).  The  index  i  ranges  over  the  set  of  all  C-functions  {/i, } 
which  define  the  C-Space  obstacle  Ott.  Wc  call  such  a  set  of  C-functions  a  family 
of  C-functions.  This  family  is  generated  by  consider:ng  pairwise  interactions  of 
features  on  the  boundary  of  A  and  features  on  the  boundary  of  B,  where  A  is  a 
convex  polyhedron  on  the  moving  object,  and  B  is  a  convex  obstacle  polyhedron. 
For  a  two  dimensional  example,  refer  to  figures  1.29-32  (chapter  l),  which  illustrate 
an  obstacle  polygon  B  with  four  vertices,  and  a  moving  polygon  A  with  three 
vertices.  For  these  two  polygons,  the  family  of  constraints  generated  corresponds 
to  all  possible  interactions  of  their  edges  and  vertices: 


f  amilyw{A,  B) 


=  (/aces(A)  X  ver  t{B)j  (j(ocr£(A)  X  faces{^)j 

—  ({«i,e2,e.-|}  x  Oi,  02,03}  X  {e{,e£,e£,ej  }). 


Each  pairing,  for  example  (e|,u[),  generates  exactly  one  C-function  /,.  In  three 
dimensions,  a  family  of  C-functions  corresponds  to  a  set  of  constraints  resulting 
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from  the  possible  interactions  of  one  polyhedral  component  of  the  moving  object, 
and  one  obstacle  polyhedron: 


/ amilyni)(A,  B)  aces(/ 4)  Xvert[B]j  (J^uer£(.4)  X  / aces(B)j  (J^edges(/l)Xedges(Z^)^. 

Of  course,  in  both  two  and  three  dimensions,  at  a  given  orientation,  only  a  subset  of 
this  family  is  applicable.  For  each  C-function  /,,  there  is  an  associated  applicability 
region  A{.  Equation  (2)  for  Ca  can  be  parsed  as  follows:  for  a  configuration  X,  for 
each  O-function  /,  such  thatX  is  in  the  domain  of  /,,  fi(X)  must  be  negative- valued 
(or  zero)  for  X  to  be  inside  the  C- Space  obstacle  Oa.  To  determine  whether  X 
is  in  the  domain  of  /,,  test  whether  the  rotational  component  of  X  is  within  the 
applicability  region  Ai- 

Next,  we  define 

F  =  JR3  X  SO{ 3)  -  CO 
to  be  the  space  of  free  configurations. 

Now,  for  each  C-function  /,,  Ai  C  50(3)  is  the  corresponding  portion  of 
rotation  space  where  /,  is  applicable.  We  construct  A ,•  as  the  intersection  of  a  set 
of  half-hypcrspaces  on  50(3): 


S0(3)  |  /\(j,-(0)  >  o)  } 


where  g}-  :  50(3)  — »  3?  is  an  applicability  constraint  function  (ACF).  A  C-function 
/,  is  said  to  be  applicable  for  a  configuration  X  =  (x,0)  if  0  6  A,.  In  this  chapter, 
we  will  derive,  and  prove,  the  form  of  the  ACFs.  Geometrically,  the  applicability 
regions  A{  are  complicated  three  dimensional  manifolds  (with  boundary)  on  the 
projective  3-sphcrc.  Their  boundaries  are  the  two  dimensional  manifolds  kergy.  ( j 
indexes  over  the  set  of  functions  used  to  construct  A{.  There  are  typically  three  or 
four  <7y,  as  we  will  see  later). 
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The  form  of  the  applicability  constraints  was  heretofore  unknown.  Many  of 
the  representational  and  algorithmic  issues  for  geometric  planning  problems  with 
six  degrees  of  freedom  rely  on  a  correct  formulation  of  the  applicability  constraints. 
With  these  advances,  however,  the  mathematical  framework  will  be  complete,  and 
we  can  construct  the  planner  of  chapter  2  which  exploits  the  geometry. 


The  work  of  Drooks  and  Lozano-Pcrcz  (1983)  dealt  with  surfaces  in  the  C-Space 
5R2  X  S\  which  arc  called  C-surfaces.  The  obvious  extension  of  this  concept  for 
6 DOF  is  a  C-manifold  in  3?'  X  50(3).  For  a  C-function  /j  we  define  a  level 
C-manifold  to  be  the  set  of  configurations  X  where  /,•  is  applicable  and  /,(X)  ~  i, 
for  some  level  l.  Thus  a  level  C-manifold  is  the  level  set  /tr,(£).  Of  particular 


interest  is  the  C-manifold 


ker/t  Wr>)  =  {X|/,(X)==0}, 


which  contains  a  boundary  patch  of  a  C-Space  obstacle.  Since  in  the  literature, 
C-manifoids  of  this  form  have  been  called  C-surfaces,  we  shall  also  employ  this 


term. 


Wc  now  define  paths  in  C-Space.  Given  a  start  configuration  a  and  a  desired 
goal  configuration  g,  a  successful  collision-free  path  is  a  continuous  function 
p  :  i1  — *•  SR2  X  50(3)  such  that  p(0)  =  s,  p(l)  “  g,  and  p(/')  C  l'  -  /’  denotes  the 
closed  unit  interval,  (0,  1]. 


3.3.  The  Geometric  Interpretation  for  C-functions 


Consider  the  interaction  of  an  obstacle  polyhedron  D  and  a  moving  polyhedron 
A,  where  both  A  and  L3  are  convex.  Let  Jp  be  in  the  family  of  C-functions  generated 
for  A  and  13.  Jv  models  a  constraint  on  the  motion  of  A.  For  example,  fp  might  be 
generated  by  considering  the  interaction  of  a  face  of  A  and  a  vertex  of  13.  For  a 
given  orientation  0,  the  projection  into  'J?'  of  any  (applicable)  C- manifold  /“ 1  (0) 
is  a  plane  corresponding  to  a  face  of  the  polyhedron  resulting  from  the  Minkowski 
sum  of  Q/l  and  D,  that  is, 


i.iw;* 


'$0 
'  w  ■ 


■m 


m 


r- 


m 


set 


m 


foil 


B  Q  A(Q)  =  {b  +  a{Q)  \  b  e  U,a  <=  QA) 


where  c(0)  denotes  vector  a  rotated  to  orientation  0  and  0A  =  {—a  |  a  C  A}. 
(Note  that  in  constructing  0A(0),  the  “negation”  takes  place  before  the  rotation). 
B  0  A(0)  is  the  projection  into  3?3  of  the  C-Space  obstacle  at  orientation  ©.  In 
effect,  we  have  parameterized  the  plane  equations  of  faces  of  J3©  A(0)  by  0.  Here 
is  the  form  of  the  parameterized  plane  equations  derived  by  Lozano-Pcrez  (1983): 
at(0)  is  a  vertex  of  0A(0)  and  bj  is  a  vertex  of  B.  Recall  that  the  equation  of 
a  plane  in  !JJ3  can  be  expressed  as  {  x  |  ( N,x )  -=  ( N.q )},  where  N  is  the  plane 
normal  and  q  is  a  reference  point  known  to  be  on  the  plane.  Then  C-functions  take 
the  form: 


/,(*.  8)  =  me),  x)  -  <JV(0),  («.(6)  +  (.,))  (4) 


where  a:  is  a  point  in  5R3.  N(Q)  is  the  real-space  component  of  the  C-manifold 
normal  at  orientation  ©,  and  is  defined  as  follows:  for  a  type  (a)  C-function, 
N(0)  is  the  normal  of  a  face  of  ©A(0).  For  a  type  (b)  C-function,  7V(0)  is  the 
normal  of  a  face  on  B,  and  hence  is  constant..  For  a  type  (c)  C-function,  iV(©)  is 
the  cross-product  of  an  edge  on  B  and  an  edge  on  0A(0).  Furthermore  iV(0)  is 
normalized  to  a  unit  vector  when  it  is  non- zero. 


The  geometric  significance  of  is  now  ciem.  The  value  oi  jD  represents 

how  far  the  (reference)  point  x  lies  above  the  plane  of  a  face  in  the  ©-parameterized 
Minkowski  solid.  (Assume  (x,  0)  €  /'’).  When  the  projection  of  x  falls  on  the  fp- face 
of  the  Minkowski  solid,  the  metric  provided  by  fp  represents  the  translational 
distance  to  a  collision.  When  the  projection  falls  outside  the  face,  the  value  of  fp 
represents  the  translational  distance  to  the  plane  of  the  /,,-face.  Hence  even  though 
there  is  no  convenient  way  of  talking  about  distances  between  configurations  in 
9?3  X  50(3),  we  can  employ  the  values  of  C-functions  as  a  metric  on  the  distances 
of  the  moving  object  from  obstacles  at  any  configuration.  This  metric  will  become 
important  in  chapter  6. 


3.4.  Redundant  Constraints 


In  chapter  2,  we  gave  an  informal  definition  of  a  redundant  constraint  (see 
figure  there).  We  now  give  the  formal  definition  of  a  redundant  constraint  for 
a  configuration  X  £  F.  Intuitively,  a  redundant  constraint  is  one  subsumed  by 
nearer,  intervening  C-functions  (lower  C-manifolds).  Let  C  denote  the  set  of  all 
applicable,  positive-valued  C-functions  at  X  =  (x,0).  For  each  /,  €  C,  let  s,-  be 
the  projection  into  SJ?3  of  the  kernel  of  /,  restricted  to  orientation  0.  That  is, 


■s,  =  {  v  e»:l  | /,(</,  e)  =  o}. 

Note  that  s,-  is  the  projection  into  SR3  of  the  tangent  hyperplane  at.  0  to  the  level 
C-manifold  for  Intuitively,  st  is  the  plane  of  the  face  of  the  Minkowski  solid 
determined  by  at  orientation  0. 

Now,  let  hi  be  the  half-space  of  JK3  bounded  by  s,-  containing  x.  Constructing 

f>. 

I 

yields  a  solid  S  in  SR3.  Those  half-spaces  bounding  S  correspond  to  the  non-redundant 
constraints  at  X. 


3.5.  Applicability  Constraints  for  type  (a)  and  (b)  C-functions 

We  are  now  in  a  position  to  derive  the  domains  of  the  C-functions.  To  define  the 
applicability  constraints,  we  consider  a  family  of  C-functions  in  isolation  (that  is, 
an  environment  comprising  only  the  obstacle  B  and  the  moving  polyhedron  A).  We 
perform  an  analysis  to  see  what  generators  can  interact  at  what  orientations.  While 
C-functions  are  defined  on  the  “negated  object”  0/1(0),  applicability  constraints 
are  defined  from  the  “positive  object”  A{0). 

Definition:  Consider  a  constraint  c,  generated  by  (ga,g b)  where  the  pair  ({7„,<7i)  is 
either  (a)  a  face  of  A  and  a  vertex  of  B,  (b)  a  vertex  of  A  and  a  face  of  B,  or  (c)  an 
edge  of  A  and  an  edge  of  B.  We  say  c  is  applicable  at  orientation  0  if  some  pure 
translation  of  A(@)  can  bring  3U(0)  in  contact  with  gt,,  such  that 


Sec  figure  3.1.  Let  /(0)  be  a.  face  on  a  moving  polyhedron  /!(©),  with  a  normal 
Af(0).  Let  bj  be  a  vertex  on  obstacle  B.  ( f,bj )  generates  a  type  (a)  constraint.  Let 
11  be  the  set  of  adjacent  vertices  of  bj  on  the  edge  graph  of  II,  that  is, 


R  =  {  bn  £  vert(B)  \  Sbtl  f)<%  7^  0  }. 

It  is  instructive  to  parse  the  definition  for  R.  (Recall  that  6  denotes  coboundary).  6bn 
is  the  set  of  edges  incident  at  6„.  If  two  vertices  bu  and  bj  have  disjoint  coboundaries, 
then  they  are  not  adjacent  on  the  edge  graph  of  B.  If  their  coboundaries  overlap, 
then  the  common  clement  is  the  edge  connecting  bn  and  bj. 

Theorem  UI.l:  A  type  (a)  constraint  generated  by  (/,  bj)  is  applicable  at  orientation 
©  if,  and  only  if,  for  all  b„  £  R, 


b„  -  N(0)  -  bj  ■  N (©)  >  0. 


(3.1) 


If  the  type  (a)  constraint  is  applicable,  then  (3.1)  holds  for  all  vert  ices  bn  of  B. 
We  will  show  that  considering  the  vertices  in  R  provides  a  necessary  and  sufficient 
condition  for  applicability. 


Proof:  (*=)  Observe  that  applicability  is  invariant  under  translation.  Wc 

* . r _ _  it  .  .... .  1 _ _  4  l  .  1  4i.  .  _  _r  rfrw  _ _ jl  .  »-ni - r_  _ 
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x  £  Ift3,  z  ■  jV(0)  is  the  perpendicular  distance  of  x  from  the  plane  of  the  constraint. 
Since  face  normals  arc  outward-directed,  when  this  distance  is  positive,  then  x  lies 
above  the  plane  of  /(0).  If  (3.1)  is  true,  then  when  bj  is  brought  to  rest  on  the 
plane  of  /(©),  then  bj  ■  /V(0)  —  0.  Now,  for  all  bn  0  R,  bu  •  iV(0)  >  0.  Thus  all 
adjacent  vertices  to  bj  are  on  or  above  the  halfspace  boundary.  Since  A  and  B  are 
convex,  their  interiors  cannot  intersect. 


(^)  If  wc  can  bring  bj  in  contact  with  /(©)  while  maintaining  the  disjoint 
interior  criterion,  then  we  have  bj  •  /V(0)  —  0.  No  6„  £  R  can  dip  below  the  surface 
of  /(©),  since  then  the  interiors  of  A  and  B  would  intersect.  Hence  each  bu  must 
lie  some  distance  d  >  0  above  the  plane  of  /(©).  I 
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Nov/,  let  /  be  a  face  of  B  with  normal  N.  (Sec  figure  3.2).  Let  c,  be  a  vertex 
of  A,  and 

R  =  {  a„  €  verl(A)  \  6an  p)  £a,  ^  0  } 
be  the  vertices  adjacent  to  a,  on  the  edge  graph  of  A. 

Theorem  HI.  2:  A  type  (b)  constraint  generated  by  (a,,  /)  is  applicable  at  orientation 
0  if,  and  only  if,  for  all  an  6  R, 


a„(6)  •  N  -  0,(0)  •  N  >  0. 

Proof:  Symmetric  case  of  Theorem  (Itl.l).  | 
Consider 


(3.2) 


9k(0)  =  bn  •  N{9)  -  bj  •  N{Q) 


(3.3) 


Figure  3.3.  Tlic  applicability  region  Ac  is  the  intersection  of  the  hnlf-hyperspac.cs  where  gt  >  0. 
as  a  mapping  <]k  :  50(3)  — *■  5R.  We  call  g ^  a  type  (a)  applicability  constraint  function 
(ACF).  (There  are  several  ACFs  for  one  type  (a)  C-function— or  indeed  for  any 
C-function,  and  they  are  indexed  here  by  k).  For  the  symmetric  case  from  (3.2),  we 

.noil 


<?*(©)  =  an( 9)  •  N  -  a,(0)  •  N  (3.4) 

a  type  (b)  ACF.  The  region  on  50(3)  where  gh  is  positive-valued  defines  a  half- 
hypcrspace  cf  S0(3)  (see  figure  3.3).  (3.2)  and  (3.1)  define  the  applicability  region 
for  a  type  (a)  or  (b)  constraint  as  the  intersection  of  these  half-hyperspaccs.  This 
yields  the  conjunction  promised  earlier: 


=  (0  6  50(3)  I  A(s*(e) 
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A  C-function  c  is  applicable  if  and  only  if  for  a  configuration  (z,  0),  each  of  c’s 
ACFs  is  positive  (or  zero)  at  ©,  that  is,  0  £  Ar.  The  number  of  ACFs  for  a  type 
(a)  or  (b)  constraint  is  equal  to  the  cardinality  of  die  coboundary  of  the  generating 
vertex  (which  is  the  same  as  |/£|). 

3.6.  Applicability  Constraints  for  Type  (c)  C-functions 

Determining  the  applicability  regions  for  type  (c)  C-functions  (generated  by 
edge-edge  interactions)  turns  out  to  be  a  bit  more  grueling.  We  can  derive  a  set 
of  ACFs  for  type  (c)  constraints  which  are  analogous  to  g ^  in  (3.3)  and  (3.4).  The 
conjunction  of  these  type  (c)  ACF3  is  a  necessary  but  not  sufficient  criterion  for 
applicability.  The  positive  conjunction  (the  intersection  of  half-spaces  where  the 
type  (c)  ACFs  are  positive)  forms  two,  disconnected  regions  in  50(3).  It  will  become 
apparent  shortly  how  these  regions  arise,  but  let  us  pause,  before  bringing  in  some 
complicated  machinery,  to  survey  their  topology.  In  one  region  A  the  type  (c) 
constraint  is  applicab’e,  in  the  other  A1,  it  is  not.  To  determine  which  region  0  is 
in,  we  use  a  set  of  related  functions  termed  disambiguating  applicability  constraints 
(DACs).  Fortunately,  the  symmetric  region  A'  where  the  ACFs  arc  positive  but 
the  constraint  is  not  applicable  is  disconnected  from  the  valid  applicability  region 
A  (where  the  ACFs  arc  positive  and  the  constraint  is  applicable)  by  a  region  A, 
where  the  ACFs  do  not  hold  (sec  figure  3.4).  Wc  will  demonstrate  that  since  A  is 
disconnected  from  A',  it  is  possible  to  plan  continuous  paths  within  A  with  heed 
only  for  the  basic  type  (c)  ACFs.  Doth  type  (c)  ACFs  and  DACs  are  functions  of 
the  form  g  :  50(3)  — *■  5ft  ;  however,  they  arc  considerably  more  complicated  than 
(3  3)  and  (3.4),  above. 


3.6.1.  The  Basic  ACFs  for  Type  (c)  Constraints 

Let  c  be  a  type  (c)  constraint  generated  by  the  pair  of  edges  (ea,e(,).  As 
usual  ea(0)  denotes  ea  rotated  to  orientation  ©.  We  will  define  type  (c)  ACFs 
which  provide  a  necessary  criterion  for  applicability.  In  conjunction  v/ith  the  DACs 
(below),  the  type  (c)  ACFs  form  a  complete  characterization  of  the  applicability  of 
type  (c)  C-functions.  We  employ  the  following  construction:  imagine  trying  to  make 
the  midpoints  of  e„  and  e$  touch  while  maintaining  the  disjoint  interior  criterion 
for  A  and  B.  We  then  allow  A  to  pivot  about 


v  =  mid(e$)  ==  mid(ea(G)) 


(3.5) 


while  maintaining  disjoint  interiors.  Keeping  (3.5),  for  what  orientations  (values  of 
0)  are  the  interiors  of  A  and  B  disjoint? 


♦ 


Figure  3.5.  A  section  view  through  (cj,  is  orthogonal  to  the  page). 

Let  us  denote  the  vertices  of  e„  and  as  follows:  (sec  figures  3.5  and  3-6) 
vert[ea )  =  (a,,  a,+1)  and  uert(ei)  —  (6y,fcJ+ j).  Now,  e0  hounds  2  faces  fi  and  on 
A]  let  their  normals  be  Ni  and  N?.  Similarly,  let  the  normals  for  the  faces  /a  and 
fi  cobounding  cj  be  N$  and  N+. 

Theorem  H1.3:  If  a  type  (c)  constraint  generated  by  (ea,e/>)  is  applicable  at 
orientation  0,  then 


-dj(©)d2(0)  >  0 

and 

~d3(0)d.,(0)  >  0 

where 

d,(9)  =  bj  •  N,(0)  -  mid(cfc)  •  N,(0) 


(3.6) 

(3.7) 

(3.6o) 


-7~rr 


Figure  3.6.  A  section  view  through  e„(9)  (ca(0)  is  orthogonal  to  the  page). 

<f2(0)  a  bj  •  N2{Q)  -  mid(c6)  ■  N2(0) 
di[Q)  —  a;(0)  •  -  mid(co(0))  •  7V3 

d4(©)  -  a,(0)  •  Na  -  mid(e0(9))  •  N4 


(3.6  b) 
(3.7  a) 


V-'0) 


(We  express  (3.6-7)  in  this  form  rather  than  as  d](0)d2(O)  <  0  in  order  to  preserve 
the  positive  sign  convention  for  all  the  ACFs). 

Troon  Refer  to  figures  3.5  and  3.6.  Again,  since  applicability  is  invariant  under 
translation,  we  transform  the  workspace  so  that  mid(e{,)  is  at  the  origin.  With 
mid(ea(0))  fixed  at  mid(efc),  c/,(0)  for  i  =  1,2  is  the  distance  of  bj  above  the.  plane 
of  /,;  for  i  =  3,4,  this  is  the  distance  of  a, (9)  above  the  plane  of  /,.  We  allow 
ea  to  rotate  about  v  —  mid(c6)  with  3  degrees  of  freedom.  Observe  that  a,  and 
a1+1  may  not  dip  below  the  surface  of  D,  and  that  bj  and  bj  may  not  fall  below 
the  surface  of  A.  This  is  clearly  enforced  by  considering  only  the  planes  of  the 


faces  cobounding  ca  and  q,.  If  the  type  (c)  constraint  is  applicable  at  ©,  then  di(0) 
arid  d;>(0)  can  never  both  be  positive,  nor  both  negative,  for  in  these  cases  e<,  will 
intersect  the  inter  ior  of  A.  Wc  see  this  as  follows:  If  <i|(0)  <  0  and  <h>(0)  <  0,  then 
bj  is  inside  both  halfspaces,  and  some  point  on  the  line  segment  (inid(ej,),  bj)  must 
be  inside  A.  if  dj(0)  >  0  and  ^(B)  >  0,  then  6J+ 1  is  inside  both  half  spaces,  and 
some  point  on  the  line  segment  (mid(fit),  6J+  [)  must  be  inside  A. 

Hence  dj(0)d2(0)  £  0<  This  immediately  yields  (3.6).  A  similar  and  symmetric 
argument  yields  (3.7).  E 


3.7.  Disambiguating  Applicability  Constraints  (DACS)  for  Type  (c) 
Constraints 

The  basic  type  (c)  ACFs  take  into  account  edge -edge  interactions,  but  do  not 
model  the  interactions  of  the  faces  they  bound.  In  order  to  preserve  the  disjoint 
interior  criterion,  wc  introduce  Disambiguating  Applicability  Constraints  (DACs)  as 
follows.  DACs  are  constraints  on  the  tangent  vectors  to  faces  cobounding  ej  and  e0; 
assuming  that  the  basic  ACFs  have  determined  that  ©  <E  A  U  A',  DACs  discriminate 
between  A  and  A1-  In  fact,  the  DACs  arc  necessary  and  sufficient  conditions  for 
applicability.  Wc  split  the  type  (c)  applicability  computations  between  the  basic 
type  (c)  ACFs  and  the  DACs  for  reasons  relating  to  the  algebra  system,  which 
is  described  in  chapter  4.  Our  proofs  draws  heavily  on  constructions  employing  a 
separating  plane. 

The  Separating  Plane  Construction 

.loin  the  midpoints  of  ea(0)  and  e/,  together  as  usual.  Consider  the  plane  P 
containing  v  =  mid(ea(0))  —  mid(cj),  whose  normal  is  ea(0)  X  e*-  Assume  without 
loss  of  generality  that  e„(0)  X  e*  7^  0.  P  contains  both  and  ea(0).  Suppose  that 
the  type  (c)  ACFs  for  constraint  c  arc  positive-valued  (or  zero),  i.e.,  (3.6)  and  (3.7) 
hold.  Hence  each  vertex  of  e 4  is  on  or  above  the  plane  of  one  face  eobounding 
ea(0),  and  each  vertex  of  e„(0)  is  on  or  above  the  plane  of  one  face  cobounding  ej,. 
Refer  to  figures  3.6  and  3.5  once  more.  Dy  reason  of  the  ACF  Values  for  c  and  the 
convexity  of  A  and  D,  some  open  halfspace  P/i  of  5J?3  which  is  bounded  by  P  must 
contain  i(/7)  entirely,  and  sonic  open  halfspacc  P\  bounded  by  P  must  contain 
z(A(©))  entirely: 

D  C  k(P,j) 

A( 0)  C  k(Fa). 

(Recall  that  k(5)  denotes  the  closure  of  a  set  S:  k(S)  ~  dS). 

Now,  if  c  is  not  applicable,  then  f(A(0))  fl  *(/?)  0-  This  means  that  A  C  «(P/j) 

also,  since  unless  P,\  —  Pj),  then  P  would  separate  z(/l(0))  from  Wc  conclude 
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that  for  all  S'  £  A',  ^(A(0'))  C  P/j  and  i(/i)  C  /'/ 1-  1?y  a  symmetric  argument,  for 
all  Q  £  A,  plane  P  separates  i(/l(0))  from  i[B).  To  summarize:  If  the  constraint 
c  generated  by  (e„ ,  e/,)  is  applicable  at  orientation  0,  then  i(/<(0))  f)  1{P)  r— 
Therefore  there  exists  a  separating  plane  between  i(A(0))  and  i(P).  On  the  other 
hand,  if  c  is  not  applicable,  there  exists  no  such  separating  plane,  for  then  the 
interiors  could  not  intersect.  Furthermore,  if  elt(6)  X  e;,  7^  0,  then  there  exists 
exactly  one  separating  plane  that  contains  all  four  points  vert(ca(S))  U  t>er£(e(,).  We 
formalize  these  results  in  tire  following  lemma: 

In  this  lemma,  we  abbreviate  ea(S)  by  ea ,  arid  A(0)  by  A. 

Lemma  111.4.1:  (Existence  and  uniqueness  of  the  separating  plane).  Join  together 
the  midpoints  of  e„  and  e^.  Assume  that  ea  X  «t,  0.  The  constraint  c  generated 

by  (<?a;ei)  is  applicable  if,  and  only  if.  the  plane  P  containing  ea  and  e*,  separates 
the  interior  of  A  from  the  interior  of  B. 

Proof:  («=)  If  P  separates  t(A)  from  f(/J),  then  z(A) f| r(P)  ~~  Therefore  the 
constraint  c  is  applicable.  ( 


Proof:  (==>)  If  c  is  applicable,  then  the.-v,  exists  exactly  one  separating  plane  between 
i(A)  and  r(Z?),  and  this  plane  is  P.  To  see  this,  first  observe  that  if  i( A)  f|  i(B)  —  0, 
then  by  convexity  there  must,  exist  some  separating  plane.  Assume  that  this  plane 
does  not  have  normal  e„  X  ei,.  In  this  case,  the  plane  cannot  contain  both  ea  and 


€/,.  Since  the  plane  contains  the  midpoints  of  both  edges,  it  must  intersect  cither 


...  _  ......  _ ii_i  ^..4.  1  >..4  4 1. :  .  - .  a.  _  4 _ . » fl. ..  4  ^  ~  c 
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either  A  or  B.  Thus  it  cannot,  be  a  separating  plane.  Since  there  must  exist  some 

separating  plane,  it  must  have  normal  ea  X  e$.  R 

The  strategy  for  defining  DA.Cs  is  as  follows.  For  each  face  cobounding  <:■ ,  we 
choose  a  point  in  the  interior  of  that  face.  The  basic  type  (c)  ACl's  ensure  that 
ei  is  outside  the  interior  of  A(0),  and  that  e„(0)  is  outside  the  interior  of  B)  flic 
DACs  ensure  that  the  faces  cobounding  e/,  iie  on  the  opposite  side  of  P  from  the 
the  f  acts  cobounding  eu(0).  If  the  type  (c)  constraint  is  applicable,  then  7,(A(0)) 
must  lie  in  a  lialf-space  bounded  by  P  complementary  to  the  half-space  bounded 
by  P  containing  z(P).  The  DACs  ensure  that  if  the  faces  cobounding  e(1(0)  lie  in 


m » 


K(f/t)i  then  the  faces  cobounding  ej,  must  lie  in  k(Pjj),  with  P \  Pq.  Since  A  and 
B  are  convex,  this  suffices  to  show  that  A  and  B  lie  in  complementary  half-spaces 
bounded  by  P. 

The  vertices  of  eo(0)  and  e*  fie  on  P.  Let  pi,  p2  be  points  in  the  interior  of  the 
faces  cobounding  c0,  and  p^,  p4  be  points  in  the  interior  of  the  faces  cobounding 
ti-  The  DACs  ensure  that  pj  and  P2  lie  on  one  side  of  P,  and  that  p 3  and  p4  fie  on 
the  other. 

The  points  inside  the  faces  cobounding  ei  and  ca  are  chosen  as  follows.  For 
each  edge  e  on  B  and  A,  wc  construct  a  pair  of  tangent  vectors,  (Tj ,  T2),  where 
T\  and  T'z  are  tangent  and  interior  to  the  faces  cobounding  e.  Tj  and  are  also 
perpendicular  to  e.  For  an  edge  ea  on  A,  (7’i(0),  T>(0))  will  clearly  rotate  with  e„ 
and  A,  maintaining  these  criteria.  The  tangent  pair  for  e &  is  shown  in  figure  3.7. 
Formally,  we  proceed  as  follows: 


Definition:  A  tangent  vector  to  SR3  (O’Neill  (1966))  is  a  pair  (v,p)  €  SR3  X  SR3, 
interpreted  as  the  vector  v  applied  to  point  p.  We  will  sometimes  write  vp  for  (o,p), 
or,  when  there  is  no  ambiguity  about  the  point  of  application,  we  simply  write  v. 

Definition:  Consider  an  edge  e  on  a  polyhedron  P.  Let  fa,  fa  be  the  faces  that 
cobound  e,  and  let  N\ ,  N2  be  their  normals.  A  tangent  pair  for  e  is  a  pair  of  tangent 
vectors  to  SR3,  (7’i,7’2),  both  applied  to  mid(e).  T,  is  perpendicular  to  e  and  to  TV,, 
and  it  is  directed  into  the  interior  of  /,  when  applied  to  mid(e)  (r  =  1,2).  In  other 
words, 


Ti  =  k{NiXe)  (t  =  1,  2) 

where  k  €  {  +1,-1  }  is  chosen  to  orient  T{  into  the  interior  of  fa.  Ni  X  e  indicates 
the  cross  product  of  N,  and  the  directed  edge  vector  for  e. 
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Refer  to  figures  3.7  and  3.8.  We  will  now  construct  DACs.  Let  be  the 

tangent  pair  for  cj,  and  let  N,t,  N4  be  the  normals  to  the  faces  cobounding  e&.  Let 
(7’i(0), 72(0})  be  the  tangent  pair  for  e„(0),  and  let  N i(0),  N2(©)  be  the  normals 
to  the  faces  cobounding  ea(0).  Thus  7'j  •  N,  =  0  (for  i  =  1,2,  3,  4).  Keeping  with 
this  numbering  convention,  let  /,•  be  the  face  with  normal  Nt.  As  usual,  we  imagine 
joining  together  the  midpoints  of  e;,  and  ea(0). 


Let  iV/>(0)  be  the  normal  to  the  plane  P,  that  is,  Ni>(Q)  =  ea(0)  X  ej.  Assume 
without  loss  of  generality  that  iV)>(0)  ^  0.  The  plane  containing  inid(e„(0))  = 
rnid(ef,)  with  normal  Np(Q)  also  contains  ctl(0)  and  e^.  Wc  construct  DACs  which 
ensure  that  i(A(0))  is  on  one  side  of  P,  and  that  i (B)  is  on  the  other  side.  To 
ensure  that  the  points  mid(e(,)  +  and  mid(et)  +  T\  lie  on  the  same  side  of  P,  we 
have  the  constraint 

s,gn (73  -  JV;>(0))  =  sign(Tj  •  Nj>(Q)) 


f  »•  L  ■  A  L  m  A  La  Itivill 
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(73  •  Afi*(0))(r4  •  N/>(©))  >  0. 


Assume  without  loss  of  generality  that  the  signs  are  non- zero.  The  case  where  one 
sign  is  zero  is  easily  handled  by  examining  the  other  sign.  To  ensure  that  the  points 
mid(ea(0))  +  7\(0)  and  mid(ea(0))  +  7'2(©)  lie  on  the  same  side  of  P,  we  have  the 
symmetric  constraint 


sign(7’,(8)  •  Nj,[e))  =  sign(T2(0)  ■  #,.(©)) 


Now,  we  must  ensure  that  the  two  half-spaces  are  complementary.  This  is  enforced 
by  insisting  that  the  signs  are  opposite.  All  of  the  following  must  be  true: 


kj,  =  sign(T;j  ■  iV;>(0)) 

=  sign(7’4  ■  Nr{0))  (3.8a) 

kA  —  s,gn(7’i(©)  ■  N]’(Q)) 

=  sign(r2(0)  •  iVH©))  (3.86) 

k a  7^-  kjj  (3.8c) 


Equations  (38a-c)  embody  the  DACs  we  require. 


Theorem  IXL4:  Let  c  be  a  type  (c)  C-function  generated  by  (ert,ei).  Assume  the 
tangent  pairs  for  e/,  and  eu,  and  normals  to  the  faces  cobounding  e 4  and  ea  are  as 
above.  Then  c  is  applicable  if,  and  only  if,  the  all  the  DACs  (3.8a~c)  hold. 

Proof:  (=t)  Assume  the  type  (c)  constraint  is  applicable,  but  that  at  least  one  of 
(3.8a-c)  is  false.  We  will  demonstrate  a  contradiction.  Join  the  midpoints  of  e„(0) 
and  ej,,  as  usual.  If  any  of  the  DACs  is  false,  then  P  does  not  separate  z(A(0))  from 
i[B):  a  contradiction.  | 

Proof:  (<=)  We  show  that  if  the  DACs  hold,  then  c  is  applicable:  if  these  conditions 
are  true,  then  P  is  a  separating  plane.  Therefore  the  interiors  cannot  intersect,  and 
c  is  applicable.  | 
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3.S.  On  the  Structure  of  the  Type  (c)  Applicability  Regions  on  SO(3) 

In  this  section,  we  prove  a  theorem  on  the  structure  of  the  regions  A,  A',  and 
A  for  type  (c)  constraints,  (sec  figure  3. 4)  which  yields  an  immediate  completeness 
result  for  our  formulation  of  ACFs  and  DACs.  As  promised,  we  will  show  that  A 
and  A'  are  disconnected  on  50(3),  and  that  the  region  A  separates  them.  Our 
proof  draws  heavily  on  constructions  employing  a  separating  plane  (lemma  111.4.1). 

Theorem  HI. 5:  A  disconnects  A  from  A 1  on  SO(3). 

Proof:  We  first  observe  that  by  definition, 


3!UUlU')  =  SO(3) 


(see  (3.6),  (3.7)  for  confirmation).  Recall  the  separating  plane  construction:  we  saw 
that  for  all  0'  G  A',  r(A(©'))  C  Pu  and  i[13)  C  P/j-  bet  Pu  denote  the  interior 
of  the  complement  of  Pj,:  Pjf  =  {(SR'1  P, j).  By  a  symmetric  argument,  for  all 
0  G  A,  plane  P  separates  i(A(0))  from  i(D),  If  >1 IJ  A'  is  path-conncctcd,  then  there 
exists  a  continuous  function,  p  :  /'  — *  50(3),  such  that  p(0)  —  0,  p(l)  =  ©',  and 
p(/’ )  C  A\JA'.  Furthermore,  if  A[JA'  is  path-conncctcd,  then  for  all  t  G  P,  either 
{(A(p(£)))  C  Pit,  or  i(A(p(<)))  C  Pis  (assume  without  loss  of  generality  that  for  all 
t,  eb  X  ctt(p(t))  0).  Note  that  for  all  t , 


pr\i(A(»(t.}))  =  0. 


Hence  in  traversing  the  path  p  in  rotation  space,  A  is  required  to  “flip”  over  P 
from  Pjj  to  P/j,  without  its  interior  ever  intersecting  P.  This  is  clearly  impossible 
if  continuity  is  to  be  preserved.  | 

3.9.  Orienting  Type  (c)  Constraints 

Consider  affixing  mid(ea(0))  to  v  —  mid(eft)  as  usual.  Refer  once  more  to  3.5 
and  3.6.  The  cross  product 

I  N,*(0)  —  e„(0)  X  eb 


lift 


I 


when  applied  to  v  will  for  some  0  point  out  of  P/t  and  into  Py\)  for  other  0, 
Nj>(Q )  will  point  into  Pn  and  out  of  P^.  (Assume  for  now  that  N/>[Qi)  0.)  lienee 

for  some  orientations  Ni>(Q)  is  the  correct  (unnormalizcd)  real-space  normal  for 
constraint  (cn,Ci);  for  other  orientations  we  must  employ  —N/>(Q).  When  applied 
to  v,  the  real-space  normal  kN/>(&)  (for  k  £  {+1,-1  })  must  always  point  out  of 
P/t  and  into  The  following  rule  for  choosing  k  is  stated  without  proof: 

k  =  sign (iV/.(0)  •  7(0))  (3.12) 


where  /(0)  =  Ti(0)  +  72(9). 

However,  it  is  easy  to  see  that  we  need  not  compute  this  dot  product  each 
time  we  use  the  C-function.  k  (and  the  orientation  of  Nj>(Q))  will  be  invariant  in 
regions  of  A  where  the  signs  of  the  ACFs  are  invariant.  For  example,  if  k  is  positive 
for  some  0  <=  A  and 

d;,(0)  >  0  and  d4(0)  <  0,  (3.13) 

then  clearly  wherever  (3.13)  holds,  then  k  must  be  positive.  Also,  v/hcrover 

d,\(6)  <  0  and  d4(0)  >  0, 

k  must  be  negative.  This  argument  should  be  quite  obvious  if  the  reader  imagines 
how  the  cross  product  of  the  edges  changes  as  ea  pivots  about  mid(t't).  This  leads  to 
the  following  simple  algorithm  for  orienting  a  type  (c)  C-function  c.  Essentially,  we 
can  just  compute  (3.12)  onc.c,  and  record  the  signs  of  the  ACFs  at  that  orientation. 

(i)  For  some  0,  compute  the  values  of  d,(0)  (i  =  1,  2,  3,  4)  for  the  type  (c)  ACFs. 
If  c  is  not  applicable,  then  stop. 

(ii)  If  k  has  not  been  computed  yet,  calculate  k  as  in  (3.12).  (Assume  k  ■/--  0).  Record 
the  signs  of  d;j(0)  and  d,j(0)  for  c.  We  call  this  pair  of  signs  the  sign  map  for  c. 

(iii)  If  a  k  and  sign  map  have  been  computed  for  c,  then  compare  the  recorded  sign 
map  to  the  current  s'gn  map  for  d,t[0)  and  d4(0).  If  the  sign  maps  are  equal, 
use  k  to  orient  c;  otherwise  use  —  k. 
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3.10.  Singularities  and  Special  Cases 

Our  analysis  of  type  (c.)  ACFs  and  DACs  assumes  that  e„(0)  and  e ^  are  never 
aligned,  i.c.,  that  their  cross-product  is  never  zero.  In  addition,  our  algorithm 
for  orienting  type  (c)  C-fuuctions  itssumes  that  no  function  d,  is  zero.  T  he  cross 
product  will  be  zero  when  eo(0)  is  parallel  to  e t,,  and  an  ACF  will  be  zero  when 
either  en(0)  is  aligned  with  a  face  cobounding  e*,,  or  when  ej,  is  aligned  with  a  face 
cobounding  e^(O).  In  practice,  these  special  cases  will  arise  frequently.  Fortunately, 
they  can  be  ignored.  Consider  the  following:  The  vertices  of  e.x  generate  type  (b) 
constraints  with  the  faces  cobounding  rq,;  and  the  vertices  of  e&  generate  type  (a) 
constraints  with  the  faces  cobounding  ea.  In  the  cases  where  e„(0)  is  aligned  with 
or  a  face  cobounding  si  (or  in  the  symmetric  case),  some  of  these  constraints 
will  also  be  applicable.  In  these  aligned  cases  we  say  that  the  type  (c)  constraint 
is  subsumed  by  the  neighboring  type  (a)  and  (b)  constraints,  because  the  disjoint 
interior  criterion  will  be  enforced  by  the  type  (a)  and  (b)  constraints  alone.  This  can 

U.t  »./»  ■»  .  / eon  fimipn  Q  onnnoen  o  A  IT'  lr\r  nvTmnIp  rl o  tv  lllpH 

l/o  ocon  a.’  iwiii'r.  y  *1^,'“  *  -  '  *  •  •  j  ■*.,  . .  *  »•  ,J  j  *«•  -*>  ;  •-  •  ”  •  —  -  -• 

at  0,  and  that  0  €  A.  Then  both  mid(ea{©))  and  a;(0)  can  be  brought  to  rest 
on  the  plane  of  f$,  while  preserving  the  disjoint  interior  criterion.  Since  a,n(0) 
is  also  lies  on  ea(0),  it  too  may  be  brought  to  rest  on  the  plane  of  /a-  Clearly, 
the  type  (b)  constraints  generated  by  (< ii,f3 )  and  (fin-1,/3)  must  also  be  applicable 
at  orientation  0.  At  this  aligned  orientation,  the  type  (c)  constraint  ensures  the 
following:  while  mi,d(eo(0))  is  on  the  plane  of  fo,  a,(0))  must  also  lie  on  the  plane 
of  fi .  This  is  precisely  the  condition  enforced  by  the  equivalent  pair  of  type  (b) 
constraints.  Symmetric  arguments  hold  for  the  other  ACFs. 

3.11.  Level  ACF* 

For  ACFs,  there  is  an  analogous  concept  to  a  level  C-Manifold.  Let  g  :  S0( 3)  -•> 
Si  be  an  ACF  for  a  C -function  r.  An  ACF  Boundary  is  the  space  of  rotations  where 
.  .  applicable  and  g  is  zero: 

kcr  j  =  {0£  SO( 3)  J  g(0)  ---  0 }. 


A  Level  ACF  is  the  space  of  rotations  where  c  is  applicable  and  g  is  some  constant 


{eesc>(3)U(e)  =  <}. 


Recall  the  geometric  interpretation  for  ACFs.  Consider  a  type  (b)  constraint  (see 
figure  3.5).  A  path  p  :  /'  — ►  50(3)  along  a  level  ACF  for  the  constraint  (a,-,/;{) 
would,  if  the  midpoints  of  the  edges  were  affixed,  preserve  a,(6)  at  a  constant 
height  above  the  plane  of  fa. 


3.12.  A  Note  on  the  Computation  and  Algebra  of  Applicability  Constraints 

The  implemented  planning  system  contains  an  algebra  system  (described 
in  chapter  4),  which  performs  the  computations  relevant  to  the  applicability 
constraints.  We  would  like  to  make  the  computation  as  simple  as  possible,  for 
otherwise  an  implementation  might  be  infeasible. 


\\T _  1.  -I _  ll..i 
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constraints; 


(i)  Type  (a)  ACFs  (3.3)  which  determine  the  applicability  of  type  (a)  C- functions, 

(ii)  Type  (b)  ACFs  (3.4)  which  determine  the  applicability  of  type  (b)  C-functions. 

(iii)  [Basic]  Type  (c)  ACFs  (3.6)  and  (3.7),  which  provide  a  necessary  but  not 
sufficient  condition  for  the  applicability  of  type  (c)  C-functions. 

(iv)  DACs  (3.8a-c)  which  provide  necessary  and  sufficient  conditions  for  type  (c) 
applicability. 


However,  it  is  not  hard  to  show  that  the  real- valued  functions  for  (iii)  and 
(iv)  are  composed  of  simple  type  (a)  and  (b)  ACFs.  We  will  demonstrate  this  ;i3 
follows.  Lei  be  the  sps.ee  of  normals  to  planes  in  -J?'1.  Note  that  X /'*  is  of  course 
isomorphic  to  Si.  We  .cow  define  the  functions  F]\  and  Fjj  to  model  the  computation 
of  type  (a)  and  (b)  ACFs.  These  functions  will  be  composed  to  compute  the  more 
coir,  pi  ex  type  (c)  ACFs  and  DACs.  Let  T\  and  F'u  be  real-valued  functions 


Fa,  Fn  :  X  3i3  X  M*  ->  3t 


where 


FA(b,„  bj,  N,  0)  =  hH  ■  N(Q)  -  b:  ■  N( 9) 


and 


Fn(a„,  ait  N,  0)  au(0)  •  N  -  <*,•(©)  •  N. 


Clearly,  Fa  and  F u  can  be  used  to  compute  ACFs  for  all  type  (a)  and  (b) 
constraints.  They  can  also  be  used  to  compute  type  (e)  ACFs  as  follows: 


FA(bjt  mid(e6),  Nit  0), 
FB(ai,m\d{ea}tNi,e), 


if  i  =  1, 2; 
if  i  =  3,  4. 


Nj‘(0)  is  already  computed  as  the  real-space  normal  for  a  type  (c)  C-function. 
With  iV/>(0)  in  hand,  DACs  can  be  computed  using  FB  and  Fa-  This  is  because 
DACs  are  essentially  constraints  on  tangent  vectors  to  the  faces  of  the  polyliedra 
in  question,  and  the  tangent  space  of  is  isomorphic  to  its  normal  space.  We 
will  show  how  to  compute  DACs  using  type  (a)  and  (b)  ACFs.  Our  trick  for 
rotating  a  tangent  vector  ( v,p )  simply  involves  rotating  the  line  segment  (p,v  +  p) 
to  (p(0),  [u  +  p](0))-  For  example, 

Ti(O)  ■  Np(9)  =  F„(mid(co(©0))  +  ^(©o),  mid(ew(0o)),  Nr{ 0),  ©) 

Here  0o  denotes  some  fixed  orientation.  Typically  Go  is  the  identity  element  for 
the  rotation  group,  i.e.,  it  denotes  no  rotation  at  all,  and  will  be  the  orientation 
in  which  the  polyliedra  aic  given,  and  in  which  the  tangent  pairs  arc  initially 
computed.  In  particular,  [7{(0o)J(0)  —  T,(0). 

Our  reduction  of  all  applicability  computation  to  a  few  simple  functions  is 
partially  motivated  by  aesthetics,  and  partially  by  the  design  of  an  algebra  system 
for  our  planner.  The  reduction  will  admit  a  simpler  and  more  elegant  design. 

3.12.1.  A  Conjecture 

Let  us  make  one  final  comment  on  type  (c)  ACFs.  For  each  type  (c)  C-  function, 
there  are  two  type  (c)  ACFs.  One  type  (c)  ACF  (3.6)  is  the  product  of  two 
type  (a)  ACFs;  and  the  other  (3.7)  is  the  product  of  two  type  (b)  ACFs.  These 
products  are  constrained  to  be  negative.  In  practice,  we  would  probably  wish  only 
to  compute  the  value  of  each  subrcsnlt  (if,-)  for  eacli  type  (a)  ami  (b)  ACF,  and  then 
compute  i  logical  conjunction  to  determine  when  one  is  negative  and  the  other 
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positive,  instead  of  computing  their  product.2  We  conjecture  that  the  composition 
of  type  (c)  C-functions  and  ACFs  reflects  the  underlying  algebraic  structure  of  those 
constraints:  observe  that  each  type  (c)  face  fub  of  the  Minkowski  solid  B  ©  A(0) 
is  the  composition  (by  direct  sum)  of  an  edge  on  A  and  an  edge  on  B:3 

fa,b  —  eo(0)  0«6*  (3.14) 

Similarly,  the  (real-space)  normal  Na>b  to  such  a  face  is  the  composition  (by  vector 
cross-product)  of  an  edge  on  A  and  and  edge  on  B: 

Na,b  =  ea{G)  X  eb.  (3.15) 

In  this  chapter  we  have  derived  a  new  symmetry,  a  symmetry  for  the  ACFs  of  type 
(c)  constraints.  In  particular,  it  is  now  clear  that  type  (c)  ACFs  are  the  composition 
(by  scalar  multiplication)  of  a  pair  of  type  (a)  or  (b)  ACFs. 


2Tliis  approach  is  taken  for  the  implemented  planner. 

3 liquation:!  (3.H)  and  (3.15)  are  from  Lozano-l’crcz  (1983). 
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4 

Mathematical  Tools  for  Motion  FlamiitfVt; 
in  a  Six  Dimensional  ConlTig vacation  Space 

4.1.  Introduction 

Our  earlier  presentation  of  representational  issues  and  applicability  constraints 
in  3?'1  X  SO( 3)  addressed  basic  theoretical  issues  for  the  motion  planning  problem. 
In  this  chapter  we  discuss  specific  issues  which  were  critical  foi  the  ir  aplc  mentation 
of  the  planning  system  described  in  chapter  2.  The  fundamental  issue  is  the 
intersection  problem  in  high-dimensional  configurat  ion  .ip^ces’. 

(i)  IIow  do  we  intersect  high-dimensioua!  level  C-Maimokh  to  construct  an 
intersection  manifold? 

(ii)  IIow  do  wo.  intersect  atrajectory  in  configuration  space  with  C-Spacc  constiaints? 
Examples  and  applications  of  these  results  may  be  found  in  chapters  1  and  2. 

We  will  proceed  as  follows.  First,  as  a  “simple”  example,  we  will  ■..••olve  these 
problems  for  the  configuration  space  X  51.  For  this  space  the  algebra  is  not 
unreasonable  and  illustrates  some  of  the  complexities  of  planning  for  the  6 DOF 
case.  However,  in  X  50(d),  the  equations  for  some  constraints  (notably,  type  (c) 
constraints)  can  fill  several  pages.  For  this  reason,  I  first  computed  the  general  form 
of  the  intersections  for  an  arbitrary  constraint,  and  then  solved  all  intersections 
using  Macsyma  (ECS  (1983)).  The  results  were  then  optimized  and  compiled  into 
Lisp.  For  all  practical  purposes  these  results  are  in  machine  readable  form  oi  ly.  For 


example,  using  Euler  Angles  parameterized  by  ©  =  {rp,0,<f>)  for  three-dimensional 
.rotations,1  a  type  constraint  in  Uaesyma  becomes: 

( (-XC  (AI )  »XC  (NGj)  'COS  (PHI)  -XC (AI)  *YC  (NGJ)  *SIN  (PHI))  *C0S  (THETA) 

‘  XC  (AI)  *20  (NGJ)  *  SIN  (THETA)  -YC  (AI)  *YC  (NGJ)  *C0S  (PHI) 

-YC (AI)  *XC (NGJ)  *SItI  (PHI)  ) 

*C0S(PSI) 

+  (  (YC  (AI)  *XC  (NGJ)  *C0z  (PHJ)  +YC  (AI)  *  YC  (NGJ)  *SIN  (PKI)  )  *C0S  (THETA) 

V-  -YC  (AI)  ‘ZC  (NGJ)  /S  'J-  (THETA)  -XC(AI)*YC (NGJ)  *C0S(PHI) 

+xc(ai)«xc(hgj:-  *  SIN  (PHI)) 

♦SIN  (PCI)  -ZC  (AT)  <  ZC  (NGJ)  ‘COS  (THETA) 

+  v-ZC  (A.I)  *XC  (NGJ)  *cos  (PHI)  -ZC  (AI)  *  YC  (NGJ)  *  SIN  (PHI)  )  *SIN  (THETA) 

-YC  (NO  J)  CCS  (PHI)  +XC  (NGJ)  ‘SIN  (PHI)  ‘ZC(NGJ)  *Z‘YC  (NGJ)  *Y‘XC  (NGJ)  *X 
-ZC  (dJ)  -'ZC  (NGJ)  -YC  (BJ)  *  YC  (NGJ)  -XC  (BJ)  *XC  (NGJ)  . 

This  is  the  simplest  of  the  constraints;  a  type  (c)  constraint  is  over  10  times  as  long. 
Foi  IR*  X  SO( 3)  our  approach  has  been  to  (l)  derive  these  constraints  (and  the 
ACFs)  from  some  arbitrary  representation  for  rotations,  (2)  reduce  each  constraint 
to  a  series  of  simpler,  canonical  forms  which  are  linear,  bilinear,  or  quadratic  in  the 
terms  of  interest,  and  (3)  develop  simple  mathematical  procedures  for  operating  on 
the  canonical  forms. 

For  example,  to  construct  an  intersection  manifold  for  n  constraints,  wc 
essentially  need  to  solve  a  set  of  n  simultaneous  equations,  each  of  the  form 

f{X)  —  0.  (X  6  X  50(3)) 

Wc  proceed  as  follows.  Let  D  =  {  x,y,  4> }  be  the  set  of  all  the  degrees 

of  freedom.  First  wc  select  F,  a  subset  of  6  —  n  dements  of  D.  P  wili  parameterize 
the  intersection  manifold.  The  variables  in  P  will  be  the  free  variables  which  the 
planner  can  choose;  the  variables  D  —  P  will  vary  dcpendently  with  P  so  as  to  stay 
on  the  i:  tcrsection  manifold.  Mechanically,  this  entails  (l)  solving  the  n  constraints 
simultaneously  eliminating  all  but  one  variable  in  D  P,  and  (2)  expressing  all 
dependent  degrees  of  freedom  D  —  P  in  terms  of  the  free  variables  J1 . 

The  canonical  forms  are  expressions  for  C-functions  which  make  explicit  the 
cocflicients  of  the  dependent  variables  (D  —  P)  themselves,  and  of  the  sines  and 
cosines  of  these  variables.  13  complicated  equations  describe  the  canonical  forms 
'liuler  iiukIcs  are  implemented  as  rotation  matrices  in  tlm  planner.  Sec  Hymon  (l!)71). 


of  a  C-function,  and  9  equations  are  needed  for  a  type  (a)  or  (h)  ACK.2  Complete 
Maesyma  listings  of  these  procedures  are  provided  in  an  appendix.  Before  wading 
into  these  waters,  however,  let  us  turn  our  attention  to  the  conGgu ration  space 

a3  x  s'. 

We  will  adhere  to  the  definitions  and  conventions  established  in  chapter  3. 


4.2.  The  Intersection  Problem  in  SR2  X  S' 


The  find-space  and  End-path  problems  in  3ft2  X  S1  are  of  considerable  intrinsic 
interest.  We  have  suggested  that  good  algorithms  for  the  two  dimensional  Movers’ 
problem  could  be  developed  by  planning  along  the  intersections  of  constraints  Some 
of  the  necessary  theoretical  tools  for  this  approach  are  presented  in  this  section. 
These  results  illustrate  the  principles  necessary  for  planning  along  intersection 
manifolds  in  2ft3  X  50(3j.  The  derivations  are  simpler  because  (1)  the  constraints 
are  simpler  and  (2)  the  applicability  regions  are  merely  sectors  on  the  unit  circle.  A 
complete,  general  path  planner  has  been  implemented  for  this  problem  (see  Brooks 
and  Lozano-Perez  (1983)1.  This  section  serves  both  as  a  pedagogic  example  and  as 
a  presentation  of  a  new  approach  to  the  planning  problem  in  3ft2  X  S'. 

To  plan  paths  along  the  intersections  of  constraints,  we  must  be  able  to  construct 
the  intersection  manifold  of  some  set  of  constraints.  To  preserve  tradition  (sec 
Brooks  and  Lozano-Perez  (1983),  for  example),  we  will  call  any  level-0  C-manifold  a 
C- surface.  A  C-surfacc  is  the  space  of  configurations  where  a  C-funclion  is  applicable 
and  zero- valued.  C  surfaces  are  interesting  because  they  bound  C- Space  obstacles. 
Wc  will  derive  the  form  cf  the  intersection  of  any  two  C-sur(aces  in  3ft2  X  S  .  Each 
C-surfacc  is  a  2-dimensional  manifold  in  3ft2  X  S'1,  and  their  intersection  manifold  is 
a  curve  p  in  5ft2  X  S'.  We  derive  a  curve  p  which  is  parametric  in  0.  Since  there  are 
2  types  of  C-surfaces  (type  (a)  and  (b)),  there  are  3  types  or  intersection  manifolds. 

•1.2,1.  The  Intersection  of  Two  C-Sur  faces  in  3ft2  X  S' 


We  describe  a  technique  for  finding  the  intersection  of  two  C-surlaces  for  the 
two  dimensional  mover’-  problem  with  rotations.  Throughout  this  discussion  of 

*VVc  :  i:»w  why  it.  w  o  ilo i— ihU-  U>  cKjtri  s.'  ;■!!  AOl's  and  I A t  -s  :u  compositions  of  type  (a) 

aim  (It)  Aw  s. 

that-  [~  y,0)  is  a  typical  jk.hu.  in  the  O- Space.  $i‘  X 


yy. 


hi'2  X  51,  we  will  employ  the  abbreviations  C  =  co3 0  and  S  =  sin 0.  The  surfaces 
are  embedded  in  a  4-dimensional  manifold  and  expressed  as  functions  on  (x,  y,  C,  S ) 
with  the  added  constraint  that  C2  +  S2  =  1.  A  system  of  equations  for  two  surfaces 
can  then  be  solved  for  x  and  y  in  terms  of  C  and  S. 

Two  type  (a)  constraint  surfaces  are  functions  of  the  form  f(x,y,0 )  =  0,  for 
example: 


sin(0  +  \)y  +  cos(0  -f  X,)x  —  ||6y J|  cos(0  +  Xt-  —  7y)  —  |jat-j|  cos(Xi  —  q,)  (ol) 


sin(0  +  Xt-)y  +  cos(0  H-  X,-)x  -  } 1 6y | [  cos(0  +  X/  -  7')  -  |(a/||  cos(Xt-  -  77,-)  (a2) 

Similarly,  two  type  (b)  surfaces  are: 

sin [4>j)v  +  cos -  || a* ||  c.os(0  -  <j>j  +  77,)  -  j|fe7-||  cos (<£y  -  7y)  (61) 


sin [4>'j)y  +  cos(<6j)x  -  ||a,'||  cos(0  -  <£y  +  77/)  -  ||fey||  cos(<£y  -  7y)  (&2) 


Refer  to  figure  (4.1).  Here  the  cq’s  are  vertices  of  the  “negated”  moving  polygon 
(QA  in  Lozano-Perez  [1981,  1983]),  in  its  local  coordinate  system.  7/7  is  the  angle 
the  line  from  the  origin  of  that  coordinate  system  to  the  point  o,  makes  with  the 
coordinate  system’s  x  axis,  and  \  is  the  angle  made  by  the  normal  to  the  segment 
from  a,-  to  a,^.  Similarly  the  fey’s  are  the  vertices  of  a  convex  obstacle  polygon,  7 y 
the  orientation  of  the  line  from  the  origin  to  6y,  and  </>y  the  orientation  of  the  normal 
to  the  segment  from  fey  to  feym-  The  parameter  0,  a  parameter  of  the  configuration 
space,  measures  the  angle  between  the  x-axes  of  the  object  and  obstacle  coordinate 
systems. 


Type  A  constraints  can  be  thought  of  as  being  generated  by  a  face  (edge)  of 
the  moving  object  A  coming  into  contact  with  a  vertex  of  an  obstacle  D,  and  a 
type  U  constraint  as  a  vertex  of  A  coming  into  contact  with  a  face  (edge)  of  li. 


\ 


Figure  8.  The  two  types  of  surfaces  can  be  defined  by  bring  tbe  reference  point  of  the 
negative  of  moving  object  A  into  contact  with  a  vertex  and  an  edge  of  fixed  obstacle  B. 
Both  are  defined  over  a  range  of  orientations  9. 


Figure  4.1.  An  illustration  «r  the  terms  in  equations  (al)  and  (bl).  Reprinted  with  permission 
from  Brooks  and  lavzano-IVrez  (1983). 
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Each  constraint  is  valid  only  over  a  fixed  range  of  0.  For  type  A  surfaces  the  range 
is  given  by  0  £  [4>j-\  —  X»,  <f>j  —  X^]  and  for  type  II  surfaces  by  0  G  —  <f>j  —  X,-  _|].4 

By  applying  trigonometric  reductions  we  can  express  these  constraints  as 
follows  (only  (al)  and  (bl)  are  shown): 

cos [\)Sy  +  C  sin(X,)y  —  sin(X,jSa;  +  C  cos(X,)x 
+  sin(Xi  -  7>)HM|S  ~  lla»'ll  cos(X*  ~  Vi) 
-Ccos(X,-7y)||hy||  (at) 


sin(0j)j/  +  cos (4>j)x  —  ||a,||  sin(^y  —  r?,)S 

-  C||at||  cos(^y  -  r}i)  -  |!fey||  coa{4'j  -  7y)  (61) 


Where 

C  —■  cos  0,  S  —  sin  9. 

Nov/,  v/c  can  consider  a  pair  of  these  equations  as  a  system  in  four  variables, 
( x,y,C,S ),  and  proceed  to  solve  (al)  and  (b2).  (bl)  and  (b2),  and  (al)  and  (bl)  for 
x  and  y.  For  example,  the  intersection  of  two  type  (a)  surfaces,  (al)  and  (a2)  is  a 
curve 

P  ■  A»l  rir<*2  -*■  9?2  X  Sl 

where  r0iflra2  C  Sl  denotes  the  intersected  applicability  constraints  for 
(al)  and  (a2).  Although  the  solutions  are  in  the  variables  C  and  S.  we  can  use 
C  —  cos0  —  cosr  and  S  —  sin<?  =  sin  r  to  generate  the  curve  of  intersection 
in  9?2  X  Sl.  Because  of  their  excessive  length,  these  equations  may  be  found  in 
appendix  I. 

4.2.2.  Intersecting  Trajectories  with  C-surfaces 

A  General  Discussion  for  UJ2  X  51  and  UJ'1  X  SO( 3) 

In  order  to  motivate  a  discussion  of  the  intersection  problem  for  trajectories 
and  C-surfaces,  we  now  introduce  the  problem  in  a  context  which  will  be  expanded 


4  Source:  The  hist,  three  paragraphs  are  excerpted  from  Brooks  and  l.ozano-i’erez,  [l'JHd]. 


upon  in  chapter  5.  The  goal  of  this  discussion  is  to  illustrate  how  the  intersection 
results  are  used  in  the  planner  described  in  chapter  2. 


Ill  principle  it,  is  possible  to  intersect  arbitrary  trajectories  with  C-surfaces — 
such  trajectories  could  translate  and  rotate  simultaneously.  Once  an  intersection 
is  found,  we  must  then  determine  whether  (1)  the  C-surface  is  applicable,  and  (2) 
whether  it  lies  on  the  boundary  of  a  C-Space.  obstacle.  The  question  of  applicability 
may  be  resolved  a  priori  by  maintaining  and  updating  an  accurate  sot  of  applicable 
constraints  as  the  planner  moves  through  rotation  space.  This  set  is  called  the 
applicability  set.  As  the  planner  moves  from  0  to  0',  the  updating  algorithm 
must  detect  which  constraints  have  expired  (ceased  to  be  applicable)  and  which 
new  constraints  have  been  activated  (become  applicable).  The  expired  constraints 
are  deleted  from  the  applicability  set,  and  the  new  constraints  are  added.  In  this 
manner  the  trajectory  will  be  intersected  only  with  the  applicable  constraints. 
Another  approach  involves  intersecting  the  trajectory  with  all  C-surfaces,  and  then 
finding  the  first  applicable  intersection  on  the  boundary  of  a  C-Space  obstacle. 
The  first  strategy  is  more  general  in  that  it  decomposes  the  image  of  the  trajectory 
into  equivalence  classes  where  the  applicability  set  is  invariant.  Hence  it  can  in 
principle  be  used  to  map  out  these  equivalence  classes  on  50(a).  However,  for 
most  environments  the  latter  strategy  runs  faster,  although  both  techniques  can 
be  shown  to  have  the  same  asymptotic  complexity.  Both  algorithms  have  been 
implemented''’  and  tested,  and  arc  presented  later  in  chapter  5. 

There  are  also  two  ways  to  determine  if  an  intersection  lies  on  the  boundary 
of  a  C-Space  obstacle.  Let  X  be  the  intersection  point  of  a  trajectory  with  an 
2,pp!ic2,b!c  C-surface  f .  Then  X  lies  qh  the  boundary  ol  &  £7- Spo.cz  obsta.de  bounded 
by  /  if  cither  of  the  following  holds: 

(i)  All  applicable  C-functions  in  /’ s  family  arc  negative  or  zero- valued  at  X.G 

(ii)  If  the  projection  of  X  into  real-space  lies  within  the  displaced  face  of  the 
Minkowski  solid  corresponding  to  the  generators  for  /. 

Correctness  Argument :  Let  us  briefly  discuss  why  (i)  and  (ii)  arc  equivalent.  The 
correctness  of  (i)  is  obvious,  since  the  C- Space  obstacle  is  constructed  as  the  Gnite 
intersection  of  half  hyperspaccs,  each  of  which  is  defined  by  a  real-valued  function 
on  C-Space.  Let  5  denote  the  face  of  the  Minkowski  solid,  and  x  the  projection  of 
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'Tor  S?'1  X  bul  not  for  Si2  X  .S'!. 

6 The  family  of  ;i  C-funrtiou  in  dcliiud  in  3.2. 


the  intersection  point  into  real-space  (i.e.,  X  —  (x,0)).  We  will  demonstrate  that 

0)  («)• 

(=»)  Suppose  (i),  but  not  (ii).  We  demonstrate  a  contradiction,  x  must  lie  on 
the  plane  of  $ ,  even  though  x  S,  since  that  is  how  the  C-functions  arc  defined 
(X  could  not  be  an  intersection  point,  otherwise).  Recall  that  the  normals  of  the 
faces  (and  planes)  bounding  the  Minkowski  solid  are  defined  to  be  outward-directed 
from  the  interior.  Since  the  Minkowski  solid  is  convex,  the  plane  of  S  bounds  a 
half-space  entirely  containing  the  solid.  If  x  is  not  within  S,  then  it  must  be  outside 
the  plane  of  some  other  face,  S',  which  shares  an  edge  with  5.  But  in  this  case, 
the  C-function  corresponding  to  S'  will  be  positive- valued:  a  contradiction. 

(*=.)  The  Minkowski  solid  is  convex.  If  x  £  S,  then  it  is  behind  (or  on)  the 
plane  of  every  other  faces  of  the  solid.  The  C-functions  are  defined  in  terms  of  the 
distance  of  x  from  these  planes,  which  must  be  negative  (or  zero).  B 

One  further  note:  suppose  that  all  intersections  with  C-surfaccs — including 
non-applicable  C-surfaces — have  been  sorted  along  the  image  of  the  trajectory  in 
C-Space.  Then  if  X  is  the  first  intersection  for  which  (ii)  holds,  then  /  is  applicable 
and  X  lies  on  the  boundary  of  the  C-Space  obstacle.  Again,  both  approaches  have 
been  implemented,  and  the  results  are  discussed  later. 

Intersecting  Trajectories  with  C-surfaces  in  9?2  X  S1 

We  will  now  present  methods  for  intersecting  pure  translational  and  pure 
rotational  trajectories  with  C-surfaces  in  9?2  X  5*.  Note  that  as  long  as  every  path 
of  interest  lies  entirely  within  open  sets  of  !R2  X  S',  then  for  every  such  path  there 
exists  a  homo  topically  equivalent  path  composed  of  “staggered”  pure  translations 
and  pure  rotations.  Wc  assume  such  paths  can  be  expressed  as  (piecewise)  linear 
functions  of  some  parameter.  Intersecting  such  a  path  with  a  C-surface  entails 
finding  the  zeroes  of  the  associated  C-fuuction  (with  respect  to  the  parameter). 

Pure.  Translational  Paths.  Note  that  (al)  and  (bl)  arc  linear  in  x  and  y.  At  a  fixed 
orientation  their  projection  into  real  space  is  a  line.  A  pure  translational  path  is 
also  a  line.  Clearly  then,  intersection  of  a  pure  translational  path  with  a  C-surface 


Pure  Rotational  Paths.  A  pure  rotational  path  is  a  linear  function  from  /'  to  51. 
Intersecting  such  a  path  with  a  C-surfacc  involves  finding  the  zeros  (with  respect 
to  0)  of  the  C-function  at  a  constant  translation.  Observe  that  C-surfac.es  (al)  and 
(bl)  are  linear  in  C  and  5,  that  is,  they  can  be  expressed  as 

EyC  -|-  E%S  +  E’s  =  0  (4.1) 

where  the  terms  I?,  (for  i  =  1,2,3)  vary  only  with  x  and  y.  The  zeros  of  (4.1)  are 
not  hard  to  find.  First  wc  note  that  (4.1)  can  be  expressed  as  a  pure  quadratic  in 
C  (or  S),  and  that  solving  a  quadratic  for  its  zeios  is  easy.  (Wc  must,  of  course, 
cheek  for  the  first  applicable  zero  which  is  on  the  boundary  cf  a  C-Spacc  obstacle). 
This  method  is  not  the  best  because  of  susceptibility  to  numerical  problems  and 
singularities.  Happily,  such  equations  arise  frequently  in  robot  kinematics;  Paul 
(1981)  describes  a  stable,  singularity-free  calculation  for  the  zeros  of  exactly  this 
form  of  trigonometric  equation. 

Practical  Note 

The  reader  will  notice  that  motion  sliding  along  an  intersection  manifold 
in  5ft2  X  5l  will  not  in  general  be  a  pure  translation  or  rotation.  We  have  not 
derived  the  results  for  intersecting  arbitrary  trajectories  with  C-surfaccs  in  5ft2  X  5l, 
although  in  principle  it  is  possible  to  do  so.  Note  that  any  such  sliding  motion 
can  be  approximated  as  closely  as  desired  by  a  sequence  of  pure  translations  and 
rotations,  and  furthermore,  any  such  “approximating”  planner  will  be  complete  (in 
the  sense  discussed  above)  if  the  “sliding”  planner  is  complete. 

Furthermore,  our  purpose  here  is  a  theoretical  analysis  in  low  dimensions 
which  still  illuminates  some  of  the  staggering  difficulties  in  5ft2  X  50(3).  As  it  turns 
out,  with  the  additional  degrees  of  freedom  in  5ft2  X  50(3),  this  turns  out  to  be 
considerably  less  of  a  restriction. 

4.3.  Related  Problems  in  !ft2  X  51 

There  arc  a  number  of  interesting  related  problems  in  5ft  2  X  51.  The  first 
addresses  techniques  for  “sliding”  along  one  geometric  constraint  (C-surfacc). 
Sliding  is  a  useful  way  to  circumnavigate  obstacles;  it  can  also  be  used  to  slide  to  an 
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intersection  manifold.  The  second  result  is  el'  use  in  the  find-space  and  coordinated 
motion  problems,  and  involve*  characterizing  the  minimum  clearance  to  a  C-surface 
in  8i“  X  5*.  Again,  these  results  ate  presented  not  only  for  their  intrinsic  interest, 
but  also  as  an  exposition  of  some  of  the  algebraic  techniques  required  and  as  an 
illustration  of  the  complications  arising  in  high-dimensional  configuration  spaces. 

4.3.1.  Techniques  for  Moving  Along  C-Surfaces  in  X  6'1 

ia  this  section  we  present  techniques  for  moving  along  a  C-Surfacc.  We  could 
imagine  using  these  methods  to  move  to  the  nearest  “edge*1  (C-Surface  intersection), 
for  example.  A  level  C-Surface  is  defined  via  a  function  f(x,  y,0)  k  for  k  constant. 
/  is  exactly  of  form  (ol)  or  (6l)  (above),  and  the  level  surface  in  Si"  X  6' 1  is  all 
points 

h  ------  {  X  €  5?2  X  r,  J  j\X)  =--  k  }, 

where  rj  C  S1  is  the  0  applicability  range  for  /. 

Define  a  hyperplane  in  5K2  X  S*  as  the  set 

P  =  {  X  €  Si2  x  i  X  •  i!  -  ~h4  }, 

where  II  =  (/q,  h.2,hf). 

We  intersect  the  level  surface  L  with  the  hyperplane  7’  to  obtain  an  intersection 
curve  p  :  7l  — »  Si2  X  5*.  The  equation  for  this  curve  for  both  type  (a)  .and  (b) 
C-surfuccs  may  be  found  in  appendix  I. 

4.3.2.  Characterizing  Clearance  to  a  C-Surfa.ee 

It  would  be  very  useful  to  characterize  the  minimum  clearance  to  a  O-surfacc. 
The  result  could  be  applied  in  the  coordinated  motion  problem  to  determine  where 
two  mobile  objects  could  possibly  interact.  In  the  find- space  problem,  wn  could  use 
clearance  information  to  maximize  the  clearance  to  a  constraint  while  placing  one 
object,  in  order  to  h  avc  room  for  another.  We  would  like  to  answer  the  question: 

*  For  a  point  l\ v  (..  9??,  at  what  orientation  is  b;y  closest  to  a  C-su.rfacer  and 
U’h<ii  is  minimum  directed  clearance  vector  at  that  oi itvloUon? 

Using  Lagrange  multipliers,  we  can  minimize  a  function  f[j,y,€)  subject  to  a 
constraint  -----  0  by  com  li ucliug  the  auxiliary  function 
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II{x,y,e,l)  =  f(x,y,0)  -  £g(x,y,0 ) 

and  simultaneously  solving  the  partial  derivatives  of  II.  In  our  case,  g  will  define 
a  C-surface,  and  /  will  be  a  distance  function.  Now,  the  rotational  dimensions 
cannot  be  treated  uniformly  in  establishing  a  metric,  so  wc  will  define  distance  in 
Euclidean  space.  Minimising  the  square  of  the  translational  distance  suffices  for 
our  purposes.  Hence, 

/(i,  1 1,0)  +  (y  -  by)2. 

Differentiating  //  gives  us  a  system  of  four  equations.  Solving  these  equations  for  x, 
y,  0,  and  £  is  not  trivial.  We  provide  the  solutions  and  their  derivation  in  appendix 
I.  (Solutions  arc  given  for  both  type  (a)  and  type  (b)  C-surfaces). 

4.4.  The  Intersection  Problem  in  !  X  50(3) 

In  this  section  we  extend  the  previous  examples  of  intersection  problems  to 
the  fi-dirnonsional  C-spaoe  l*?3  v  V0(3).  At  this  point  wc  must  commit  ourselves 
to  a  particular  representation  for  rotations.  The  implemented  planner  uses  a 
rotation  matrix  specified  by  Euler  Angles.  Implementing  a  different  representation 
for  rotations  (such  as  spherical  angles,  quaternions,  or  joint  angles  for  a  Cartesian 
Manipulator)  would  merely  require  replacing  the  Macsyna  rotation  abstraction 
ROTATE-VECTOR  with  the  appropriate  new  function  (and  recompiling  the  algebra 
system).  The  Euler  Angles  are 


e  =  (V>,«?.# 

The  intersection  problems  in  ill'5  X  50(3)  are  as  follows.  Witli  each  problem! 
we  give  the  motivation  for  attacking  it. 

(i)  Intersecting  (level)  C- .surfaces.  (Necessary  to  construct  the  intersection  manifold). 

(ii)  Intersecting  Level  ACEs.  (Interesting  theoretical  question:  relates  to  planning  on 
different  kinds  of  intersec  lion  manifolds,  and  exploiting  coherence  in  C-b'pacc 
constraints). 

(iii)  Intersecting  O-Surfaces  with  Level  ACEs.  (Same  as  (»))• 

(iv)  Inti  rsccting  Trajectories  with  C-surfaces.  (Indicates  that  wc  may  have  hit  a 
C-Sparc  obstacle). 


)(>:! 


(v)  Intersecting  Trajectories  with  ACFs.  (Indicates  that  a  constraint  has  expired 
(ceased  to  be  applicable)). 

Note  that  we  never  have  to  intersect  a  trajectory  with  a  DAC,  since  any  path 
straying  out  of  a  type  (c)  constraint’s  applicability  region  must  first  violate  an  ACF 
boundary  (see  Theorem  III. 5).  Since  all  ACFs  can  be  composed  out  of  type  (a)  and 
(b)  ACFs,  we  need  only  deal  with  three  distinct  kinds  of  functions  on  X  5 0(3) 
and  two  on  50(3).  In  the  context  of  this  section  the  term  ACF  is  used  to  refer 
only  to  the  basic  type  (a)  and  (b)  ACFs  out  of  which  all  ACFs  and  DACs  may  be 
composed. 

Our  approach  is  as  follows:  We  express  all  C-functions  and  ACFs  in  certain 
canonical  forms.  The  Macsyma  procedures  to  derive  these  forms  arc  provided  in  an 
appendix.  We  then  develop  certain  operations  which  are  defined  on  any  function 
expressed  in  these  forms.  Throughout  this  discussion  of  5ft3  X  50(3),  wc  use  the 
notation  Ca  =  cos  a  and  5«  =  sin  a  where  a  £  {i Most  of  the  claims  in 
this  section  should  be  self-evident  when  the  rotation  matrix  £(0)  for  Euler  Angles 
is  considered. 

Claim  .{■ 1 •'  All  C-functions  are  affine  in  x,  y,  and  z.  This  is  obvious,  since  £(0)  13 
a  linear  transformation.  | 

Claim  4-2:  While  expressions  for  C-functions  and  ACFs  can  contain  cross- terms  of 
the  form  CnSft,  5q5/j,  or  C(1Cp,  it  should  be  clear  that  a^/i,  that  is,  Cn  can 
always  be  expressed  as  an  affine  function  of  5tt. 

To  derive  this,  consider  the  definition  of  a  C-function  (equation  (4)  in  chapter 
3)  once  more: 


fp{x,  ©)  --=  (iV(0),  x)  -  (MG),  (a,(0)  +  bj)) 

Oidy  the  term  (/V(0),  a,(0)}  cc  aid  result  in  any  troublesome  terms.  For  a  type  (b) 
constraint,  N(0)  is  a  fixed  vector.  For  a  type  (a)  constraint,  iV(0)  is  a  rotated 
normal  of  a  face  of  A,  and  wc  have 


(N(0),  «,(©))  =  <fV,  a,). 
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Finally,  for  type  (c)  constraints,  N[Q)  is  the  cross  product  of  e„(0)  and  e b.  This 
results  only  in  cross-terms  of  different  angles: 


{at(e),e„(0)  X  eb)  =  (a,(0),  ^a,n(0)  -  a,(0)j  X  eb) 

=  (ax(0),  n,-(- i(G)  X  eb  -  a,(0)  X  eb) 
=  (a,(0),at  l  j(0)  X  eb) 

=  (e6,a4(0)  X  a,+  i(©)}. 


A  proof  for  the  ACFs  is  very  similar.  | 

4.4.1.  Canonical  Forms  for  C-functions  and  ACFs 

Definition:  The  Linear  Form  for  a  C-function  /  :  3J3  X  50(3)  — *■  9?  is  an  equivalent 
expression 

f(x,  y,  z,  0)  =  Ei  x  +  E±y  +  Ejz  +  E4 , 


where  Ei  :  50(3)  ->  3?  (for  i  =  1,  2, 3,  4). 


t\  p  .!i*  a  rp..:  _  . . i„:.  _  _i_.  _  1.* .  r? _  /7’nnl  _ 

i^umihiuiii  /v  xiiyufujincLiiL,  ^uuuoua  i  u/m  {  ±  j  iui  «l  v-iuiit,iiuii  j  ta 


an  equivalent  expression 


/(*,  y,z,ip,0,<f>)  =  I\  sin  <f>  +  F-z  coo  <p  +  Fa, 


where 

F{  :  9?3  X  (Vfi  0)  -*•  3J.  (t  =  1,  2,  3) 


Definition:  A  Trigonometric  Quadratic  Form  (TQFy  (in  <f> )  for  an  ACF  g  :  50(3) 
3?  is  an  equivalent  expression 

0,  <f))  =  G 1  sin  <p  +  G2  cos  <p  +  O3 , 


where 

O,  :(iM)-».  {i  1,  2,  3) 

The  TQFs  are  defined  here  in  <j>  —  of  course  we  must  also  define  the  TQFs  in  rp 
and  in  0  in  the  natural  way.  <p  will  be  our  typical  example  angle  in  this  discussion, 
however. 
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Before  we  proceed  let  us  provide  some  intuition  for  these  definitions.  Imagine 
deriving  a  linear  form  for  a  C-function,  and  setting  the  expression  equal  to  zero. 
The  result  is  just  an  expression  whose  coefficients  make  explicit  how  the  plane 
equation  of  the  face  of  the  Minkowski  solid  changes  with  rotation. 

A  TQF  (in  <p)  is  just  a  way  of  expressing  C-functions  and  ACFs  in  terms  of 
the  coefficients  of  sin  <p  and  cos  <p.  Linear  forms  and  TQFs  will  be  useful  canonical 
forms  for  the  intersection  problem  in  O?3  X  50(3).  ft  is  important  to  realize  that  the 
coefficients  F,-,  F,,  and  Ot  are  actually  functions  on  the  other  degrees  of  freedom. 


t  Uip  XPfr  \  j  \ 


We  see  immediately  from  claims  (4.1)  and  (4.2)  that: 

Claim  4.3:  Every  C-function  can  be  expressed  as  a  linear  form  and  as  a  TQF  in 
ip,  0,  and  <p ;  similarly,  every  ACF  can  be  expressed  as  a  TQF  in  ip,  0,  and  <p. 

4.4.2.  Intersecting  C-sur faces  in  5J?3  X  50(3) 

When  intersecting  O-su rfar.es  in  5f?2  X  51,  we  essentially  eliminated  variables  in 
a  system  of  equations.  This  corresponds  exactly  to  "spending”  degrees  of  freedom 
to  comply  to  two  constraints.  In  SJ2  X  51,  there  were  few  choices  for  which 
variables  to  eliminate.  However,  in  IR3  X  5' 0(3),  we  have  many  more  degrees  of 
freedom,  and  hence  there  are  more  choices  for  how  to  solve  the  intersection  of 
a  set  of  constraints.  For  example,  to  construct  the  intersection  manifold  of  three 
constraints,  we  could  spend  all  the  translational  degrees  of  freedom,  which  would 
result  ia  parameterizing  the  intersection  manifold  by  ( xp,0,<p )■  Alternatively,  we 
could  in  principle  eliminate  the  rotational  degrees  of  freedom  and  parameterize 
the  intersection  manifold  by  (x,y,z).  In  the  former  case,  we  leave  [\p,0,<p)  as 
independent  degrees  of  freedom:  parameterizing  the  intersection  manifold  simply 
involves  solving  the  3  constraints  simultaneously  for  x,  y,  and  z  in  terms  of  (t p,0,<p). 
To  move  aiong  their  intersection,  we  are  free  to  plan  any  values  for  (ip,  0,  <p),  and 
the  parameters  for  the  translational  degrees  of  freedom  will  vary  so  as  to  comply 
to  the  simultaneous  set  of  constraints.  Obviously  the  choice  of  which  degrees  of 
freedom  should  parameterize  an  intersection  manifold  is  important;  linear  forms 
and  TQFs  give  us  a  general  way  of  attacking  it.  This  approach  is  best  illustrated 
through  the  following  examples: 
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Example  ( t ).  A  C-surface  in  linear  form  is  an  expression  for  a  C-function  in  linear 
form  set  equal  to  zero.  Two  C-surfaces  expressed  in  linear  form  may  be  intersected 
to  yield  a  4-dimensional  intersection  manifold  parameterized7  by  (z,ip,0,<f>).  This 
amounts  to  simultaneously  solving  the  equations 

f(x,  y,  z,  9)  =  Ei  x  +  EiV  +  E%z  +  E\  =  0 

g{x,  y,  z,  0)  =  E[x  +  E!2y  4-  E^z  +  E[  =  0 

by  first  eliminating  x  and  then  solving  for  y.  This  yields  expressions  for  x  and 
y  in  terms  of  (z,  0,  <£);  we  say  that  <f>)  form  a  4-parameter  family  for 

the  intersection  manifold,  and  that  x  and  y  comply  to  the  O-surfaces  /  and  g  as 
[z ,  ip ,  0 ,  <f>)  are  varied. 

This  intersection  has  the  following  geometric  interpretation.  Imagine  holding 
orientation  constant  at  ©i.  Then  J5,  and  E[  are  all  constant  also.  Intersecting  / 
and  j  at  a  constant  or  ientation  is  equivalent  to  intersecting  two  planes  in  3?'*.  The 
intersection  is  a  line,  and  the  position  along  the  line  may  be  parameterized  by 
the  one  remaining  translational  degree  of  freedom,  z.  The  planes  intersected  are 
exactly  the  planes  of  the  faces  of  the  Minkowski  solid  for  /  and  g  at  orientation 
Oi. 

Example  (ii).  The  intersection  manifold  f(X)  —  ff(X)  =  0  from  example  (i)  may 
be  intersected  with  another  C-surface,  h(X)  =  0,  expressed  in  linear  form.  Suppose 
z  is  eliminated.  Then  the  intersection  manifoid  for  /,  g,  and  h  is  parameterized  by 
(?/>,  6, 4>).  The  translational  degrees  of  freedom  x,  y,  and  z,  will  be  expressed  in  terms 
of  the  rotational  degrees  of  freedom,  and  will  slide  along  the  intersection  manifold 
as  rotations  arc  chosen.  The  new  intersection  manifold  f[X)  —  g(X)  —  h(X)  ~  0 
is  a  3-dimcnsional  sub-manifold  of  3i '  X  50(3).  This  intersection  has  the  following 
geometric  significance.  Imagine  holding  orientation  fixed  at  0]  once  more.  The 
intersection  at  a  fixed  orientation  of  /,  g,  and  h  is  the  intersection  of  three  plane? 
in  Si1.  This  intersection  (if  it  exists)  is  a  typically  a  point.  If  G|  is  allowed  io  vary, 
the  intersection  point  moves.  The  coordinates  of  the  intersection  point  are  the  x, 
y,  and  z  degrees  of  freedom  as  they  comply  to  the  intersection  manifold. 

'Assume  that  the  constraints  are  not  parallel,  and  that  this  is  possible,  etc. 
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Examples  (i)  and  (u)  show  how  to  spend  translational  degree:,  of  freedom  to 
intersect  C-surfaces.  In  (i),  we  saw  that  u  is  possible  to  plan  motion  along  the 
the  4-dimcnsional  intersection  manifold  with  one  translational  and  three  rotational 
degrees  of  freedom,  (i)  can  be  used  to  plan  pure  translational  path  complying 
to  two  C-surfaces.  The  free  translational  parameter  may  essentially  be  chosen  to 
maximize  progress  in  a  search  algorithm.  This  is  precisely  how  one  “local  expert” 
in  the  implemented  planner  works. 


One  last  note  on  linear  forma:  the  discussion  and  examples  above  can  be  easily 
generalised  to  arbitrary  level  C-surfaees  (instead  of  C- surfaces  with  level  0)  by 
increasing  or  decreasing  E4  (the  "constant”  term  in  the  linear  form)  by  a  constant 
equal  to  the  level. 


Internetting  Two  TQFa 

Consider  a  TQF  g  (in  <fA  for  either  a  O-functioa  or  ai>  ACT/  and  suppose 
further  that  the  TQF  has  been  set  equal  to  zero  so  that,  it  is  actually  a  TQF  surface, 
kero,  by  which  we  near,  a  TQF  fot  a  C-surfaco  ov  AGF  boundary. 


F\  sin  4<  -t-  F'2  cos  <f>  -(-  F3  0 
Such  a  TQF  can  be  expressed  aa: 


( F'\  +  F 2)  cos2  <p  +  '2F>1<3  co3  +  F§  -  F2  -= 


{■i.Z) 


The  new  expression  is  quadratic  in  cos <f>.  (This  explains  the  name  TQF).  The 
procedure  for  intersecting  two  quadratics  is  well  known.5  Such  a  procedure  can 
be  used  to  intersect  two  quadratics  of  form  (4.2)  (i.c.,  with  cos<^  treated  as  the 
quadratic  variable).  Thus  we  can  obviously  intersect  any  two  TQF  surfaces.  This 


means  that  the  procedure  for  intersecting  two  quadratics  can  be  applied  to  TQFs  of 

O-surfaccs  ami  of  ACF  boundaries.  This  immediately  yields  an  effective  procedure 

"Depending  on  whether  the  Tl)F  is  a  C  fnnetion  or  A  OF,  I  lie  fum. lions  l<\  will  have  different 
domains,  hut  this  will  not  matter  To;'  our  discussion. 

"For  example,  .s<-c  Winston  ami  Horn  (i!>81),  ([).  175). 
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for  constructing  the  intersection  manifold  of  two  C-surfaces,  two  ACF  boundaries, 
or  a  C-surface  and  an  ACF  boundary  while  spending  only  rotational  degrees  of 
freedom. 

4.4.3.  Intersecting  Trajectories  with  C-Surfaces  and  ACF  Boundaries  in 
3J3  X  50(3) 

In  this  section  we  extend  the  method  of  (4.2.2)  for  intersecting  arbitrary  linear 
pure  translational  and  pure  rotational  trajectories  with  C-surfaccs  in  3?'*  X  50(3). 

Pure  Translational  Paths.  A  pure  translational  path  can  not  stray  out  of  an 
applicability  region.  It  is  not  hard  to  intersect  a  linear  pure  translational  path  with 
a  C-surface.  Such  a  path  can  be  represented  as  a  line  in  At  the  fixed  orientation 
of  the  path,  any  C-surface  can  be  represented  as  a  plane  in  3i3.  Hence  the  problem 
of  intersecting  a  C-surface  with  a  (linear)  pure  translational  path  is  simply  the 
problem  of  intersecting  a  line  with  a  plane.  The  linear  form  of  any  C-surface 
directly  provides  the  coefficients  of  this  plane  for  any  (applicable)  orientation. 

Note  that  in  intersecting  a  pure  translational  trajectory  from  some  configuration 
X  €  F  with  a  set  of  applicable  C-3urfacea,  we  need  only  consider  C-functions  which 
are  positive- valued  at  X. 

Pure  Rotational  Paths.  We  restrict  our  attention  to  linear,  pure  rotations  in 
one  rotational  direction  (i.e.,  in  ± 0 ,  for  example, 

~  ko  kit  (t  G  fl) 

(for  some  constants  ko  and  fcj).  To  intersect  such  a  path  with  a  C-surfaco  (or  ACF 
boundary),  we  simply  find  the  zeros  of  the  appropriate  TQF.  For  this  example,  we 
would  use  the  TQF  (in  ^)  for  the  C-surface: 

F’i  sin  >p  -f-  /'2  cos  <j>  -h  —  0.  (4.3) 

With  motion  strictly  in  the  functions  F;  will  be  constant,  and  may  be  regarded 
simply  as  the  coefficients  of  a  quadratic  form.  (4.3)  is  easily  solved  for  the  values  of 
<f>  which  are  its  roots  (see  section  4.2.2).  Now;  depending  on  the  solution  technique, 


(4.3)  may  yield  several  roots.  The  correct  root  may  be  chosen  as  follows:  for  a 
C-surface,  we  choose  the  first  root  where  the  C-surface  is  applicable.  For  an  ACF 
boundary,  we  choose  the  first  root  where  the  associated  C-surface  is  applicable. 
This  last  step  requires  examining  the  other  ACFs  for  the  C-surface. 

Completeness  and  Complexity  for  Rotational  Trajectories 

We  have  seen  that  a  continuous  path  through  rotation  space  can  be  approximated 
as  closely  as  desired  by  a  series  of  linear  motions  along  the  rotational  axes.  We  now 
show  that  the  number  of  path  segments  required  grows  linearly  as  the  resolution 
of  the  approximation  becomes  finer. 

Definition:  Let  V  be  a  vector  space,  and  P  and  P'  be  trajectories  in  V.  Wc  say 
that  P'  approximates  P  at  resolution  r  if  for  all  p'  G  P',  the  perpendicular  distance 
of  p'  to  P  is  less  than  y. 

Proposition  4-4-  A  linear  trajectory  in  a  vector  space  can  be  approximated  by  a 

number  of  path  segments  along  the  axes,  which  increases  linearly  as  the  resolution 
1 - c - 

UCOUllIUO  UUCi  , 

Proof:  Suppose  V  —  3?3,  and  P  is  a  linear  trajectory  from  u  to  v.  Imagine 
approximating  P  by  linear  motions  along  the  x,  y,  and  2  axes.  Segment  P  into  k 
subpaths.  From  u,  attain  each  of  the  k  —  1  subgoals  (and  v)  by  3  linear  motions 
(along  x,  y,  and  2)  from  the  previous  subgoal.  This  yields  a  sequence  P'  of  3 k 
motions  which  approximates  P  at  resolution  r*.  We  can  bound  ^  from  above  as 
follows: 


^  <  -max^u*  -  14*1,  |v„  -  uy\,  \vz  - 


rk 

To  achieve  a  particular  resolution  r,  it  is  easy  to  choose  the  smallest  k  satisfying 
the  reverse  inequality.  We  sec  immediately  that  k  varies  linearly  with  r.  | 

Let  the  angle  space  Q 3  be  the  domain  of  a  chart  for  50(3),  as  described  in 
chapter  2.  Then  the  angle  space  trajectory 

p(  t)  —  ©j  +  tv 
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v  =  aij)  4-  b9  +  c<j> 
specifies  a  well-defined  trajectory  R(p(t))  in  50(3). 

Proposition  4-5:  We  can  approximate  p  as  closely  as  desired  by  a  sequence  {<?t}  of 

*1  AAA  f 

motions  in  Q 6  along  the  i p,  0,  and  <j)  directions.  Furthermore,  the  si2e  of  the  set 
{  Qi }  grows  only  linearly  as  the  resolution  r  becomes  finer. 

Proof:  Immediate,  from  proposition  (4.4).  | 

4.5.  The  Algebra  System 

The  treatment  here  of  the  implemented  algebra  system  is  mercifully  brief. 
Given  the  discussion,  the  details,  at  least  in  principle,  should  be  easily  imagined 
by  most  readers.  In  computer  algebra  these  problems  are  well  understood,  and  the 
system  does  not  make  a  significant  contributiou  to  that  field.  I  would  like  to  note, 
however,  that  the  algebra  system  is  both  massive  and  at  the  heart  of  the  planning 
system.  It  takes  12  hours  for  a  dedicated  VAX  to  optimize  and  compile  the  vector 
form  of  the  constraints  (in  Macsyma)  into  the  primitive  functions  of  the  Lisp  algebra 
system.  On  top  of  these  primitives  is  built  a  more  abstract  system,  which  (for 
example)  can  evaluate  constraints,  intersect  constraints,  intersect  trajectories  with 
constraints,  and  find  zeros  of  constraints.  The  intersection  and  evaluation  system 
has  automatic  singularity  handling  (for  division  by  zero,  imaginary  roots,  alignment, 
etc).  For  example,  to  intersect  two  C-surfaces  (a  Id  example  (i)),  the  planning  system 
will  typically  specify  a  list  of  preferences  for  the  translational  parameterization  of 
the  intersection  manifold.  The  system  then  attempts  to  construct  an  intersection 
manifold  with  a  high-ranked  parameterization,  and  on  encountering  singularities 
will  back  up  and  try  again. 

I 

|  It  should  now  be  clear  how  the  algebra  system  for  the  planner  is  designed, 

i  For  each  kind  of  constraint  (C-funclion  or  ACF),  the  algebra  system  contains 

'  procedures  which  compute  the  coefficients  of  the  linear  form  (for  C-surfaces  only), 

!  and  coefficients  of  the  TOFs.  Each  of  these  procedures  can  be  thought  of  as  a 

t 
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function  of  (1)  the  constraint,  and  (2)  the  parameters  not  explicit  in  the  form  (for 
example,  the  rotation  parameters  for  the  x  coefficient  of  a  linear  form).  On  top 
of  this  is  built  a  level  of  abstraction,  so  that  for  example  the  operation  “compute 
the  sin  <p  coefficient  of  the  TQF  (in  <}>)’’  is  defined  on  all  constraints.  Coefficients 
of  all  possible  forms  are  described  by  a  total  of  12  coefficients  for  the  linear  forms 
of  C-functions,  27  coefficients  for  the  TQFs  of  the  C-functions,  and  18  coefficients 
for  the  TQFs  of  the  ACFs.  (These  functions  correspond  exactly  to  the  functions 
E{,  Fi,  and  G,-,  above).  All  of  these  functions  are  constructed  and  optimized  by 
Uaceyma  running  under  NIL  (Burke  (1983)),  and  then  converted  into  Lisp. 


Wc  have  also  experimented  with  precompiling  functions  for  all  possible 
intersection  manifolds  (up  to  some  degree).10  For  intersection  manifolds  of  degree 
2  or  3,  this  is  not  hard,  and  in  fact  we  have  already  illustrated  all  the  necessary 
mathematics  in  this  chapter.  Intersection  manifolds  of  higher  degrees  may  be 
construct  ed  by  solving  for  the  submanifold  representing  the  simultaneous  satisfaction 
of  several  constraints,  for  example,  three  constraints  in  linear  form  together  With 


two  TQFs  such  as  (4.2).  When  higher  degrees  are  considered,  this  becomes 
quite  complicated,  especially  when  we  allow  different  parameterizations  of  the 
intersection  manifolds.  Construction  of  intersection  manifolds  of  higher  degree  may 
be  easier  when  different  representations  for  rotations— such  as  unit  quaternions — are 
employed.  This  appears  a  fruitful  direction  for  future  research.  In  practice,  we  view 
it  as  preferable,  wherever  possible,  to  obtain  the  values  of  coefficients  of  a  form 
at  a  certain  configuration,  and  then  to  plan  locally  while  keeping  these  coefficients 
fixed.  Thus  for  example,  we  might  compute  the  coefficients  of  the  linear  forms  of 
two  C-surfaccs  at  a  given  orientation,  and  then  intersect  the  resulting  planes  to 
obtain  a  translational  path  along  their  intersection.  The  structure  of  the  forms 
makes  this  easy  to  do.  For  example,  rotating  the  moving  object  (say,  in  <j>)  until 
it  hits  a  constraint  is  mathematically  a  complicated  operation.  All  we  need  do, 
however,  is  find  the  coefficients  of  the  TQF  in  <j>,  and  supply  them  to  a  procedure 
in  the  algebra  system  which  finds  the  zeros  of  TQF  surfaces.  (But  see  chapter  5  for 
the  details  of  the  applicability  set  computation). 


lflTlie  dajrrc  of  an  intersection  manifold  is  simply  ttio  number  of  constraints  intersected  there. 
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4.6.  Related  Issues  in  Si3  X  SO{ 3) 


4.6.1.  Normals  to  C-surfaces 

Let  /  be  an  applicable  C-function  and  X  a  configuration  on  a  level  C-surfacc 
for  f.  When  an  appropriate  inner  product  is  defined  on  the  tangent  space,11  the 
normal  to  the  C-surfacc  at  X  is  the  gradient  of  the  C-function  /  evaluated  at  /. 
Normals  to  C-surfaces  are  of  great  importance  for  motion  planning.  The  gradient 
may  be  computed  as  follows:  first  the  coefficients  for  the  linear  form  of  /  (evaluated 
at  X)  are  obtained: 

f{X )  =  E\x  +  E^y  +  E3Z  +  E\. 

Clearly,  —  Ei,  =  E 2,  and  ^  =  E3.  To  obtain  the  partial  derivatives  in  the 

rotational  direction,  we  find  then  coefficients  of  the  TQFs  (evaluated  at  X) 

f(X)  —  Fi  sin  tj>  +  F-2  cos  <f>  4-  F$ 

to  obtain 

df  ,  . 

— •-  —  F 1  cos  <p  —  Bin  <p. 
d(f> 

4.6.2.  C-functions,  Potential  Fields,  Penalty  Functions,  and  Morse  Theory: 
A  Conjecture 

A  popular  approximate  algorithm  for  collision  avoidance  places  “potential 
fields”  around  the  obstacles  (either  in  real  space  or  in  some  C-  Space),  and  attempts 
to  navigate  the  reference  point  through  a  trough  of  least  resistance.  The  obstacles 
may  be  thought  of  as  having  a  “charge”  which  repels  the  robot,  and  the  goal  has  an 
inverse  charge  which  “attracts”  it.  The  potential  field  method  is  closely  related  to 
the  so  called  “Morse  Theoretic”12  approach  to  motion  planning,  and  lends  itself  to 
fast  control-loop  algorithms  which  can  exercise  real-time  dynamic  control  of  robot 
arms  with  few  degrees  of  freedom,  in  simple  environments.  As  might  be  expected, 
the  method  works  best  for  robots  that  can  be  approximated  by  points  or  spheres.  A 
proper  potential  function  increases  as  the  robot  approaches  the  obstacle,  and  goes 
**See  sec.  2.1.2  iiiid  Krdmann  (1981). 

,2WI  iir.li  takes  its  name  from  Morse  Theory  in  differential  topology. 


to  infinity  at  the  obstacle  boundary.  Traditionally,  the  potential  function  is  chosen 
somewhat  arbitrarily,  with  much  emphasis  on  the  closeness  of  the  “fit”  of  the 
potential  surfaces  about  the  real-space  obstacles,  and  with  understandable  concern 
for  the  computability  of  such  functions  by  specific  control  hardware.  With  the 
theoretical  tools  we  have  developed,  it  is  now  possible  to  give  a  potential  function  in 
configuration  space  which  is  “exact.”  For  a  configuration  X,  let  /  be  a  C-function 
representing  the  maximum,  applicable,  non-rcdundant  constraint  from  one  family. 
For  each  such  /,  we  conjecture  that  a  good  potential  field  function  would  be: 

p(x)  =  /(W*'  if  /(x>  >  °- 

l°o,  if/(X-)  =  0.  . 

for  some  k  >  2.  Whether  or  not  such  penalty  functions  could  be  used  in  devising 
a  fast  real-time  control  algorithm  is,  of  course,  another  question.  The  suggestion  ia 
primarily  intended  to  show  that  there  is  a  representation  on  which  (in  principle) 
less  approximate  potential  fieiu  methods  might  be  based. 
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5 

Moving  Through  Rotation  Space 


5.1.  Introduction 

In  this  chapter  wc  discuss  some  of  the  computational  issues  involved  in 
planning  paths  involving  three  dimensional  rotations.  The  primary  issue  is  that 
of  keeping  track  of  which  constraints  (O-functions)  are  applicable  as  orientation 
changes.  In  principle  it  is  possible  to  intersect  paths  with  all  ACF  boundaries,  and 
thus  to  determine  which  applicability  regions  the  path  traverses  and  crosses.  It  is 
also  possible,  in  principle,  to  compute  the  applicability  regions  a  prion,  before  the 
planning  begins.1  In  practice  this  is  computationally  infeasible.  Even  for  simple 
environments,  there  are  typically  thousands  of  constraints,  each  of  which  has  at 
least  3  associated  typo  (a)  and  (b)  ACEs.  We  will  investigate  alternative  strategies 
which  exploit  coherence  in  how  the  set  of  applicable  functions  changes  as  the 
robot  moves  continuously  through  rotation  space.  In  previous  chapters  (particularly 
chapter  4)  we  showed  how  to  intersect  trajectories  with  C-surfaces  and  ACEs. 

The  applic. ability  set  for  an  orientation  0  is  the  set  of  all  applicable  constraints 
(C-functions)  there.  Cleaily,  there  are  regions  on  50(3)  for  which  the  applicability 
set  is  invariant;  orientations  in  the  interior  of  these  regions  correspond  to  orientations 
where  no  edges  or  faces  of  the  robot  are  aligned  with  the  edges  or  faces  of  any 

'This  approach  is  si  lilar  to  (In'  critical  region  compulations  suggested  by  Schwartz  and  Sharir 
(IJ1H1). 


175 


obstacle.  For  a  fixed  o  station  0,  we  coinp:iU;  the  applicability  set  by  examining 
the  signs  of  the  ACi's  for  all  C-funclions  (see  chapter  3).  However,  this  is  clearly 
not  an  operation  we  wish  to  repeat  very  often,  and  the  applicability  set  calculation, 
procedure  should  be  mcinoizeti.  (A  memoized  procedure  record:,  the  answer  for  a 
giver,  input,  to  it  will  not  have  to  be  recomputed.  Instead,  it  car.  simply  he  looked  up 
in  a  table).  As  the  robot  moves  in  rotation  space,  certain  constraints  will  expire  as 
the  path  moves  out  of  their  applicability  region,  and  other  constraints  will  become 
at  live  as  we  move  into  their  applicability  region.  This  suggests  that  an  incremental 
update  algorithm  should  be  possible:  we  imagine  detecting  when  constraints  expire, 
and  when  new  constraints  become  active  and  constructing  a  Detetelist  of  expired 
constraints  and  an  Add  list  of  new  constraints..  The  applicability  set  is  then  updated 
by  me  ans  of  the  Deletelist  and  Addlist. 

5.2.  The  Applicability  Decomposition  for  50(3) 

Ir.  this  chapter,  wc  will  first  present  a  name  algc.ithtn  which  does  r«ot  use 
an  update  strategy.  Wc  then  present  a  more  sophisticated  procedure,  called  the 
G  update  algorithm,  which  is  an  incremental  update  strategy.  Wc  have  performed 
experiments  using  both  algorithms  to  implement  tire  local  operator  Rotate,  which 
was  discussed  in  chapter  2.  Both  algorithms  have  the  same  asymptotic  complexity. 
Although  wc  have  applied  both  to  the  find-path  problem,  they  arc  designed  for 
fundamentally  different  tasks.  The  naive  algorithm  is  specialized  for  a  particular 
find-path  operator,  while  the  update  algorithm  is  a  general  too!  for  computing  a 
decomposition  of  C-Spacc  for  spatial  planning. 

The  §  update-  stiategy  addresses  the  fundamental  problem  of  applicability  set 
computations  in  a  continuous  space.  Without  the  Q  algorithm,  there  exists  only 
the  '‘discrete”  applicability  set  computation,  which  given  one  point  in  50(3)  can 
determine  the  set  of  all  applicable  O-functions.  With  an  incremental  update  sti  ategy 
we  can  map  out  regions  on  50(3)  for  which  the  applicability  set  is  invariant.  The 
boundaries  of  these  regions  arc  ACF  boundaries,  bet  1/(0)  the  applicability- 
set  at  G  0  50(3),  and  ~  be  a  binary  relation  on  50(3)  such  that  0  G 1  if, 
and  only  if  1/(0)  —  F(0f)-  Clearly,  ~  is  an  equivalence  relation  on  50(3),  and 
50(3)  is  decomposed  by  ^  into  disjoint  equivalence  classes  where  the  applicability 


sot  is  invariant.  We  call  this  the  applicability  decomposition  for  ,.90(3).  Computing 
this  decomposition  is  a  fundamental  step  in  reducing  continuous  spatial  planning 
problems  to  discrete  computational  problems. 

We  will  show  how  vo  compute  these  decompositions  for  section:,  of  90(3)  in 
any  of  the  directions  £  —  {iV’.i ±$}.  In  particular:  The  incremental  update 
strategy  computes  a  projection  of  the  applicability  decomposition  onto  a  subspace 
of  90(3)  which  is  isomorphic  to  S*.  In  principle  it  is  not  hard  to  generalise 
these  sections  to  arbitrary  rotational  slices:  algebraically  this  entails  solving  the 
intersection  of  a  TQF  with  an  arbitrary  pure  rotation.  As  we  have  noticed,  any 
rotation  of  interest  can  be  .approximated  as  closely  as  desired  be  a  sequence  of 
rotations  in  S ,  with  no  loss  of  completeness  (at  a  given  resolution). 

The  naive  algorithm,  on  the  other  hand,  is  highly  specialized  to  the  particular 
problem  of  rotating  to  a  constraint.  It  does  not  address  the  more  fundamental 
problem  of  dpconinnsmg  6'0('l)  into  applicability  sc  equivalence  regions.  We 
believe  that  the  applicability  decomposition  is  also  important  to  planning  problems 
other  than  find-path,  particularly,  for  lind-space,  Cue-motion,  and  planning  with 
uncertainty.  However,  in  practice  the  naive  algorithm  has  proved  faster  for  rotating 
to  p,  constraint  than  any  incremental  algorithm  we  have  devised.  It  is  gratifying  to 
find  that  both  strategies  have  the  same  asymptotic  complexity;  however,  we  have 
no  strong  indication  that  the  0( A-  log  Ar)  bound  we  demonstrate  is  optimal,  and 

f-_*  __  _  l - l _  ^  . 

iibuti  ajHia  n  <r\iav. 

5.3.  A  Naive  Algorithm  Without  an  Update  Strategy 

We  begin  by  presenting  a  naive  algorithm  for  moving  in  rotation  space  which 
does  not  employ  an  update  strategy.  We  wish  to  design  ail  effective  procidurc  which 
is  to  be  given  a  start  configuration  s,  a  goal  direction  a  C  {  f  </',  --ip,  0,  —0,  -t  4',  ~‘<P  }■ 

and  a  goal  configuration  Th"  goal  configuration  differs  from  s  only  in  that  the 
goal  angle  in  the  a  direction  will  be  >ju  instead  of  The  procedure  determines 
if  the  robot  can  reach  <j  aloug  the  trajectory  in  a,  or  whether  it  will  strike  an 
obstacle,  in  which  ease  it  must  return  the  ( ’-surf.-i.re  hit  and  tin:  intci section  angle. 
(Vie  use  intersection  anylr  u»  mean  the  value  of  <*  at  the  intersection  point). 


1 

i 

Figu'c  5.1. 

Let  C  be  the  net  of  all  C-surfaces.  Calculate  the  intersection  of  the  trajectory 

p(t)  —  s  +  ia 

with  every  C-surface  ir.  C  (whether  applicable  or  not)  Each  such  intersection  can 
be  expressed  ns  a  single  angular  value  (i.e.,  the  value  of  t  or  a  for  which  p{l) 
lies  on  the  C-surfacc)  and  hence  as  a  single  point  on  the  unit  circle.  Wc  can 
order  the  intersection  points  by  their  intersection  angle  with  a  C-surface.  Sort  the 
intersections  around  the  circle.  Then  traverse  the  intersections  on  the  circle  in 
direction  o  from  s,  and  find  the  Erst  intersection  which  is  both  applicable  and  on 
the  boundary  of  a  OS  pace  obstacle.  Ir.  4.2.2  wc  gave  an  algorithm  for  how  this 
may  be  determined. 

6.f .  Update  Strategics:  Example 

Wc  now  proceed  to  describe  how  an  update  strategy  works.  If  constraints  could 
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Figure  5.2.  As  tlic.  hammer  rotate*  in  the  —$  direction  from  (V"u,  fa)  tc  'V\j,0,vs).  the  bo*e* 
in  the  lower  left  show  Lite  C-Sj'atr.  obstacle  boundaries  and  A\,‘F  iKmndarica  that  the  trajecvory 
hits.  Since  the  hammer  hi  in  frce-apacc,  it  hit*  no  C-aurfr,C'».  However,  it  rr<HM»  many  AC F 
boundaries. 

expire  and  become  active  “arbitrarily”,  this  problem  might  si  ill  be  formidable. 
However  the  following  observation  makes  things  much  eashr: 

Claim  (5.1):  When  a  constraint  expires,  another  “aeighburing”  constraint 
becomes  active. 

For  example,  consider  figure  5.1,  which  depicts  a  cross- seclia-h  t«l  a  recta  bgioid  A 
moving  above  an  obstacle  face  /.  As  A  translates,  it  rotates  in  direction  >p  In 
5.1a,  constraint  (bi,/)  is  applicable,  and  (vu,f)  is  not.  At  5.1b,  however,  we  move 
out  of  the  applicability  region  for  (t»i , /)  and  becomes  active.  5.1b  is  v?n 

the  boundary  of  the  applicability  regions,  and  both  constraint?  ;s\'  applicable.  Dy 
5.1c,  however,  (vj,/)  has  expired.  has  replaced  {oj,/)  iu  the  appikahih&y 

set.  ( v-2,f )  clearly  seems  like  a  neighboring  constraint,  to  (u»,/)t  is  tput  v,m  ai  d  vj> 
aic  adjacent  vertices  on  the  edge  graph  of  A.  Wc  would  like  to  devise  sn  update 
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strategy  which,  given  a  Dcletelist  of  expiring  constraints,  could  enumerate  a  small 
list  of  candidates  for  the  Addlsst.  In  general  an  expiring  constraint  will  be  replaced 
by  neighboring  constraints.  However,  the  neighborhood  function  is  somewhat  more 
complicated  than  In  this  simple  example.  For  instance,  imagine  that  A  were  rotating 
towards  the  viewpoint  (out  of  the  page),  leading  with  vertex  v.i  (see  figure  5.4).  It 
is  possible  for  constraints  (i-'a, /},  (iq,/),  and  (ws,/)  to  replace  (vi,/),  if  the  faces 
/  and  {  v3,  }  arc  parallel  when  expires,  Clearly  03  is  also  “near”  vj, 

but  not  as  near  as  v-j  and  V4.  To  exploit  claim  (5.1),  it  remains  to  be  seen  just 
what  we  mean  by  a  “neighboring  constraint.”  We  should  emphasize  that  the  update 
strategy  does  not  predict  exactly  which  constraints  will  become  active,  but  merely 
a  set  of  candidate  constraints,  some  of  which  must  replace  the  expiring  constraints 
in  the  applicability  set. 

5.5.  Using  Update  Strategies 

Let  us  modify  the  naive  algorithm  to  incorporate  an  update  strategy  At 
configuration  9,  we  compute  the  applic&l  iity  set.  The  trajectory  p  is  next  intersected 
with  all  C-aurfaccs  in  the  applicability  s.  't,  arid  with  all  ACF  boundaries  for  these 
C-surfaces.  The  two  lists  of  intersections  are  merged  and  sorted  around  the  unit 
circle.  (The  sort  l  ey,  once  more,  is  the  intersection  angle).  We  call  this  sorted 
structure  of  C-surfacc.  and  ACF  intersections  the  intersection  queue,  since  it  a 
priority  queue  containing  intersections.  An  evt,Vy  in  the  internet  tion  queue  is  a  pair: 

C-surfacc  or  ACT,  Angle  of  inter  section 

We  then  traverse  tin.  intersection  queue  in  order  from  s  in  direction  a,  taking 
the  following  actions  when  we  encounter  a  C  surface  or  an  ACF  intersection: 

(1)  When  an  ACF  boundary  i;  bit,  a  C-surfacc  has  expired.  I.*t  the  angle  of 
intersection  be  Sometimes  severe!  C -surfaces  expire  at  once;  in  this  case  their 
ACF  toutiO/iniL:  will  ali  have  the  stmuc  intersection  angle  on  the  circle.  Determine 
all  the  C-sur*ates  that  expire  at  O'/  (vimpiy  scan  down  the  intersection  queue  until 
an  in  to  neutron  angle  gi  cater  then  (with  respect  to  direction  d)  is  found).  These 
C-sui faces  constitute  tin  Deietcli.i  Assume  we  have  an  update  procedure,  which 
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can  determine  an  Addlist  of  newly  active  C-surfaces  given  a  Dcletelisl  of  expiring 
O-surfaces.  Cali  the  update  procedure  with  the  Dcletelisl,  to  determine  the  Addlist. 

(i)  Delete  all  C-surfaccs  in  i.iie  Dcletelisl  from  the  Applicability  set. 

(ii)  Delete  all  O-surfar.es  in  the  Deletelist  from  the  intersection  queue. 

(iii)  Delete  all  the  ACFs  of  C-surfaces  in  the  Delctclist  from  the  intersection  queue. 

(iv)  Create  an  Addlist  intersection  queue,  i.e.,  a  sorted  structure  containing  the 
intersections  of  all  C  surfaces  in  the  Addlist.  and  all  ACF  boundaries  of  these 
C-surfaces,  with  the  trajectory  p. 

(v)  Merge  the  Addlist  intersection  queue  with  the  old  intersection  queue. 

(2)  When  encountering  a  C-surface  intersection,  we  know  the  C-surface  must 
be  applicable,  since  wo  have  not  yet  hit  an  ACF  boundary  which  could  invalidate  it, 
(This  is  essentially  the  correctness  criterion  maintained  by  step  (l)  of  the  algorithm). 
Test  to  sec  if  the  intersection  is  on  the  boundary  of  the  C-Space  obstacle.  Note 
that  this  operation  typically  requires  knowing  the  applicability  set. 


We  then  continue  traversing  the  intersection  queue  (of  course,  resuming 
traversal  the  next  a  slightly  beyond  a /  in  the  c*  diiection)  until  cither  an  obstacle 
is  hit  or  the  goal  angle  is  reached.  As  the  intersection  queue  is  traversed,  steps 
(1)  and  (2)  are  performed  to  update  the  queue  and  detect  collisions  whenever  an 
ACF-boundary  or  C-surface  (respectively)  is  crossed.  50(3)  is  typically  quite  dense 
in  ACF  boundaries:  see  figure  5.2.  In  this  figure,  the  small  boxes  depict  one 
dimensional  slices  (isomorphic  to  S  )  of  rotation  space  in  the  —0  direction.  The 

4l.in  1 1  n  •>  fttfl  />»•  fl  tn  >T  .VII  (  rtf  4  rt  nir.vlrt  1  *1  zj  1  rt  n  f  rtr>  4  h  rt  nl  n  IV  w »  ,» jrt  Vil  Vi  I  /I  k  » ft - .  O  A  *) 
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radians,  and  the  heavy  line  extending  out  of  the  circle  indicates  the  goal  angle, 

which  is  0  ~  0.  The  intersections  of  the  trajectory  with  C-surfaces  are  shown  in  the 
left  box  (there  are  none).  The  intersections  of  the  trajectory  with  ACF  boundaries 
are  shown  in  the  right  box.  Each  line  indicates  the  angle  ol  intersection  for  an 
ACF  boundary.  The  applicability  set  is  invariant  between  intersection  points.  The 
moving  object  is  shown  rotating  between  the  start  and  goal  angle.  The  C-surfaces 
and  ACl's  were  generated  by  the  moving  object  and  obstacles  shown.  However,  the 
actual  size  of  each  Addlist  is  usually  small.  The  algorithm  works  by  maintaining  a 
correct  applicability  set  as  we  move  in  a,  and  by  modifying  the  intersection  queue 
to  remove  C-surface  and  ACF  intersections  that  arc  not  applicable. 
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Ii.fi.  Update  Strategics 

In  this  section  we  finally  discuss  specific  update  strategies.  An  update  strategy 
has  two  parts:  first,  given  a  Delete!'  it  cl  expiring  constraints,  it  must  predict  a 
set  of  C-funclions  guaranteed  to  contain  the  Addlist.  Second,  it  must  test  each  of 
these  predictions  to  determine  which  are  really  applicable.  The  latter  operation  h 
conceptually  trivial,  but  since  it  is  expensive,  we  wish  to  make  the  prediction  set 
as  small  as  possible.  For  example,  predicting  C,  the  set  of  all  C-functions  is  clearly 
correct,  but  not  very  useful. 

A  better  approximation  would  be  as  follows:  given  a  Deletciist,  determine 
all  the  C- families  (i.e.,  families  of  G-sur faces)  it  represents.  A  safe  prediction 
would  comprise  aii  the  C-functions  in,  these  famiucx,  since  clearly  an  expiring 
constraint  will  ht  replaced  by  another  constraint  from  its  o\v r<  family.  In  practice 
this  approximation  has  proved  useful,  however,  it  is  not  tire  best  we  can  do.  In 
particular,  note  that  even  two  cuboids  will  gvin.-iate  to  typo  (a.)  C-sm  faces,  -i"  type 
(b)  C  surfaces,  and  }.4-i  t> pe  (c.)  C-surfaces.  Clearly  the  O -family  approximation  is 
not  a  very  tight  upper  bound  for  the  replacement  set,  that  is,  the  Addlist.  for  a 
Dclctolist. 

Lot  X'b  i  and  //>  denote  tius  vertices,  edges,  and  faces  of  polyhedron  P.  Toi 
a  moving  polyhedron  A  and  an  ob-trc.ie  polyhedron  B,  wt  can  express  the  family 

-•>!  pnn.-'1  »  niiilo  t. U - 

uUllvl  V)k  Ll4M 

(3 A  x  "V,,)  UO;/t  X  Af)  Wa  X 

To  be  foi  ir.ah  this  should,  strictly  speaking,  be  cornu  do  red  t  he  don  ain  of  a  tijcr.bon 
G  which  maps  pairs  of  gene’ .dors  to  the  1  unction  space  of  C- function but 
wiiore  thr.  is  no  ambiguity  v/c  w;!i  speak  of  a  pai:  (;/, \:<j<s)  x;>  representing  the 
corresponding  C -function  C{(Ja,  <Jh)- 

For  an  expiring  C-functior.  {<M  ;  ,!i: ),  wo  would  like  ti.  define  a  itciglihorhiind 
map  on  a  polyhedron  P, 

I8i 


$  :  V,U<V-US>  -*0VUf/‘U3>)* 


(where  *  is  the  Kle-one  star  denoting  closure)  such  that  the  set 

S'm)  x  iH.an)  (s.i) 

is  the  smallest  maximal  replacement  set  for  In  other  words,  we  want  (5.1) 

to  contain  all  possible  replacement  sets  for  ( ,  gn),  no  matter  what  the  rotational 
motion;  but  we  also  wish  (5.S)  to  be  as  small  as  possible  so  as  to  minimize  the 
ACF  computations.  It  is  possible  for  0  to  be  local  in  character:  although  several 
constraints  in  a  family  mo.y  expire  simuli-aneously,  all  that  we  require  is  that  the 
union  ot'  their  replacement  seto  is  correct. 

We  conjecture  it  might  be  possible  to  find  exact-or  at  least  smaller  replacement 
sets  by  taking  the  specific-  motion  into  account.  Such  a  strategy  has  not  yet  been 
developed,  however. 

fi.fi. i.  Mathematical  Prcliwtm&rbiS 

In  chapter  wc  gave  an  informal  definition  (by  example)  of  the  boundary  and 
coboundary  operators.  We  now  define  and  employ  two  related  operators  which  can 
be  composed  to  define  operators  such  as  "the  faces  which  contain  vertices  tq,  02, 
and  v^’  and  "the  edges  whioh  are  incident  at  the  vertices  of  these  faces.” 

In  this  section  we  define  the  discrete,  boundary  and  coboundary  operators. 
Consider  a  finite  collection  of  cells,  S.  The  discrete  boundary  and  discrete  coboundary 
u»  8,  denoted  <?S  and  6S,  are  defined  as  follows: 

dS  =  jj  ds 

■jQS 

6S  «=  {)  Ss. 

*ts 

The  disc  :ete  boundary  and  coboundary  operators  liavc  very  different  properties 
ftom  the  noimal  boundary  and  coboundaiy  operators.  For  example,  if  /  is  a  face, 
then  d~j  —  0,  while  0?f  —  vcrt(f).  To  sec.  this,  observe  that 
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*V  -  %Pf) 

=  U  de 

cC:B/ 

=  vcrt{f). 

In  fact,  for  any  ‘  wet!  behaved”  object  P  (and  in  particular,  any  polytope),  d2P  —  0 
and  62P  -----  0  (this  is  a  fundamental  theorem  of  topology).  However,  two  (or  more) 
applications  of  the  discrete  boundary  cr  coboundary  operator  will  not,  in  general, 
yield  0. 

Fxa,npic.a:  <i  (ulf  v%,  v3)  is  the  set  of  faces  F  which  contain  at  least  one  of  the  vertices 

v«,  t>2  or  tn.  Since  for  one  face  /.  d  f  ~  vtrt(f),  t  hen  3  F  —  d  6  («ift/2,  V3) 

is  the  vertices  of  all  the  faces  j’f\  The  set  of  edges  incident  at  these  ve-rticcs  is 
■  »  2 » 2 

6D  S  (vltV2tv3). 

rr> _ _  • _ iirL.i  -- 

J.ui.r.  t  1.  t.’JC  V  j  41  <-.t.  15>  v  w  {•*)  .  J'l ,  xr.y j ■ 

.Elementary  IlevJow:  Boundary,  (Jobmmdary,  and  Star 

We  must  show  that,  the  discrete  boundary  and  coboundary  operators  are  well 
bchavc-J.  We  will  do  so  by  pry  tenting  a  formal  definition  of  3  (and  6)  on  a  single 
chain.  Headers  who  have  encountered  a  bit.  of  homology  will  find  the  demonstration 
transparent.  Others  may  wish  to  take  this  section  on  faith,  and  to  skip  to  the  next 
section,  where  we  define  the  otru.  operator. 

Discrete  boundary  and  coboundary  operate!:,  can  be  considered  as  the  ordinary 
boundary  and  coboundary  "modulo  orientation.”  We  see  this  as  follows.  (For  a 
more  comprehensive  account  see  any  textbook  on  elementary  topology,  for  example, 
Hockhi  j.  arm  Young  (1061)). 

Lot  K  he  an  arbitrary  oriented  complex  of  abstract  cello,  and  2  an  arbitrary 
(addit ivoly  written)  abelian  group.  An  u-dimensiou&l  chain  on  the  complex  K  with 
codli  dents  m  .?  its  I'uncuou  c„  mapping  oriented  n  cells  of  K  to  Z,  ouch  tl  at  if 
c , . ( -t n n )  then  c„(-cn)  —  ~z.  An  arbitrary  n  chain  cu  on  K  can  be  written  as 

the  formal  linear  combination 

If!  4 


where  Z{  =  c„(+rr").  The  boundary  operator  d  is  a  mapping  from  n-cliains  to 
(n  —  1)  chains.  d[zi  •  <r“)  is  an  (n  —  l)-chain  which  has  non- zero  coefficients  only  on 
the  (n  —  l)-faces  of  the  cell  o'f.  Formally,  let  \on,<Tn~x\  be  the  incidence  number  for 
an  and  crn~ l,  that  is 


0, 

3 

1 

yV""1]--  +i, 

if  a"  1 

-i, 

if  an~x 

Kcnce, 


is  not  a  face  of  on, 

is  a  positively-oriented  face  of  an, 

is  a  negatively-oriented  face  of  crn. 


atz.-  V  hr".  <r"  "M  .  *,  .  nn~l, 

V  *  *  /  /.  i  J  » 

^n-1 

To  factor  out  the  effect  of  orientation,  we  dcGno  the  discrete  boundary  operator  as 
follows: 


h*  ■  °i)  =  EIK.'-'II 

Discrete  coboundary  is  dcDned  analogously. 

The  Star  Operator 

Let  P  be  a  polyhedron.  Any  cell  k  is  a  face  of  itself,  although  it  is  not  a  proper 
face.  A  proper  face  of  P  must  be  lower  in  dimension  than  P :  If  an  n-dimcnsional 
cell  k  is  on  the  boundary  of  P,  i.icn  we  call  1.  an  proper  n-face  of  P.  Thus  edges 
are  proper  1- faces,  and  vertices  proper  0-fac.es  of  a  3-diincnsional  polyhedron.  Let 
K  be  some  complex  of  cells.  If  k  is  a  n-facc  of  K ,  then  wc  write  K  >  k.  We  will 
usually  assume  that  a  face  is  a  proper  face. 

Now,  let  E  be  some  set  of  cells  in  K .  The  star  of  E  (in  K)  is  defined  by 
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St(S,  K)  =  {a  e  K  I  (3  r  e  E),  a  >  r}, 


i.e.,  the  set  of  all  cells  in  K  that  contain  a  member  of  S  in  their  boundary.  When 
there  is  no  ambiguity  we  will  simply  write  St(£).  (Giblin  (1977),  Ilocking  and  Young 
(1961)). 


A.  0  A  ]  A  A  2  A  A 

For  a  cell  k,  define  6  k  ==  k,  <5  k  =  6k,  and  6  k  =  5(<5k),  (etc).  We  see 

immediately  that  the  star  of  {k}  may  be  computed  as 


St({  k  })  =  (J  S' k. 

i=0 

Using  this  observation,  we  have  implemented  the  star  operator  by  recording  the 
boundary  and  coboundary  of  each  cell  in  the  geometric  model. 

5-0.2.  Local  Computation  of  Replacement  Sets 
Type  (a)  and  (b)  Constraints 

Consider  figure  5.3.  (vi,  /)  denotes  a  type  (b)  constraint.  Consider  any  rotational 
motion  from  the  configuration  shown.  Assume  this  rotation  will  cause  (t»|,/)  to 
expire.  We  wish  to  determine  the  maximal  possible  type  (b)  replacement  set  for 
(v[,  /),  that  is,  the  set  of  neighboring  type  (b)  constraints  which  could  replace  (t'i,  /) 
under  any  conceivable  rotation. 

Consider  the  set 

^(Su,) -{«,})  X  {/}.  (5.2) 

A 

=  S t>i  is  just  the  edges  which  meet  at  i>[.  The  discrete  boundary  of  these 
edges  is  simply  the  collection  of  their  vertices.  V|  is  deleted,  since  it  is  expiring. 
Now,  consider  a  rotational  motion  which  causes  («|,/)  to  expire.  (5.2)  will  contain 
replacement  type  (b)  constraints.  However,  (5.2)  is  not  maximal:  consider  a  rotation 
which  causes  (uj,  /)  to  expire  at  some  orientation  at  which  a  face  f  containing  vi 
is  parallel  to  /.  (See  figure  5.4).  Then  all  the  vertices  of  /'  are  replacements  for  V|, 


loti 


Figure  5.3. 

that  is,  the  type  (b)  replacement  set  is  Tt{J')  —  { X  {  /  }  •  In  general,  the 
maximal  predicted  type  (b)  replacement  set  for  (t/j,/)  is 
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By  similar  analysis,  we  see  the  following  claims: 

Claim  (5.3):  When  ( v\,f )  expires,  so  will  at  least  one  type  (c)  constraint  with 
generators  in 

X  Of- 


Claim  (5.4):  See  figure  5.5.  When  a  type  (c)  constraint  (e„,e$)  expires,  so  must 
some  type  (b)  constraint  with  generators  in 

A  A 

dca  X  tfej,. 
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Figure  5.4.  (a)  A  is  Totaling  above  fao-  /,  out  of  the  page,  (towards  the  eyepoint).  f  is  the 
visible  fare,  with  vertices  t,,  vx,  v3l  and  v«.  (b),  (c)  show  a  section  through  /'  as  A  rotates. 
When  /  and  /'  arc  parallel,  v2,  vj,  and  t.q  all  nocoine  active  as  t>i  expires.  This  is  a  singular 
point;  as  A  continues  to  rotate,  t>2  and  t»,  expire,  and  t'3  remains  applicable.  The  instantaneous 
replacement  set  for  «;  i.->  vcrt(f')  —  {v|  }. 

This  ar-alysix  is,  of  course,  symmetric  for  type  (a)  constraints.  In  this  case,  / 
would  be  interpreted  as  a  face  cf  A  and  iq  as  an  obstacle  vertex.  The  equations 
given  ali  work  when  the  generator  pairs  are  reversed. 

Claims  (5.3)  and  (5.4)  are  particularly  interesting,  in  that  they  suggest  that 
we  can  detect  all  expiring  type  (c.)  constraints  by  examining  the  AOFs  of  type  (u) 
and  (b)  constraints  alone. 

5.6.3.  Definition  of  the  Neighborhood  Mapping  for  the  Replacement 
Generators 


The  replacement  set  in  (5.3)  makes  a  certain  amount  of  sense:  the  replacements 
for  an  expiring  generator  tq  are  to  be  found  in  the  faces  containing  tq.  On  a 
polyhedron  P,  the  general  neighborhood  function  £  is  a  simple  generalization  of 


Figure  5.5.  Section  through  e*.  dea  =  {vi,va}. 

(5.3): 

g[  )  =  St(veri(),  dP), 

that  is,  p(k)  is  the  set  of  all  cells  which  contain  vertices  of  k  as  faces. 

Let  P  be  a  Deletclist.  The  smallest  maximal  replacement  Eet  for  P  is 

U  5{9a)  X  $(gn)- 

(tA,gn)eD 

This  particular  formulation  requires  that  we  ignore  “nonsense”  pairings  such 
as  all  members  of  "Vx  X  Vu-  This  is  easily  accomplished  by  appropriate  construction 
of  the  function  C  mapping  pairs  of  generators  to  the  function  space  of  C-functions. 
V-'b  extend  the  domain  of  C  to  dA  X  dli,  and  map  all  generator  pairs  except  those 
m  X  V:,)U(Tx  X  ?h)(J[£a  X  Sli)  to  0. 


.V.Vl'i 


A  Correctness  Proof  for  Q 

We  shall  now  argue  that  Q  is  the  correct  mapping  to  predict  smallest  maximal 
replacement  sets-  Let  {g/yt9n)  he  an  expiring  constraint  at  some  orientation  ©.  The 
replacement  set  for  a  constraint  is  the  Cartesian  product  of  the  the  replacement 
sets  for  its  generators.  Hence  the  the  replacement  set  for  (gA,gn)  is  Kff/O  X  r(gn)- 
Consider  the  replacement  set  r(g^)  for  gji,  i.c-,  the  set  of  constraints 


Kva)  X  {  gu  } 

which  will  replace  (<7ai<7»)-  This  set  of  constraints  will  become  active  at  orientation 
0,  while  (tfAj  gu)  expires.  Let 

G  = 

Note  that  (1)  all  constraints  in  G  X  {  gn  }  must  be  applicable  at  orientation  0,  and 
(2)  0  must  lie  on  the  boundary  of  each  of  their  applicability  regions  in  SO(3).  We 
say  that  at  0  each  g  £  G  is  in  ACF  boundary  condition.  Note  further  that.  r[g a) 
is  not  a  predictive  replacement  set,  but  any  actual  replacement  set  for  a  generator 
</A  under  some  arbitrary  rotation. 

We  will  first  show  that  all  g  C  G  are  coplanar. 

All  ACFs  arc  defined  in  terms  of  a  contact  vertex  and  an  applicability  vertex 
(sec  chapter  3).  The  contact  vertex  is  brought  to  rest  on  some  applicability  plane 
(which  is  parallel  to  a  face  of  the  other  polyhedron),  and  the  applicability  vertex 
is  constrained  to  lie  above  that  plane.  When  a  constraint  is  in  ACF  boundary 
condition,  then  both  the  contact  vertex  and  the  applicability  vertex  of  at  least  one 
of  its  ACFs  are  constrained  to  lie  on  the  applicability  plane.  (Wc  consider  type  (c) 
constraints  to  be  composed  of  four  such  ACFs,  two  of  which  are  type  (a)  and  two 
or  which  are  type  (b)  ACFs).  In  addition,  observe  that  each  line  segment 

^Applicability  vertex,  Contact  vertex 


too 
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lies  on  some,  edge  of  either  polyhedron  A  or  polyhedron  B.  In  fact,  these  edges, 
which  we  term  applicability  edges,  cover  the  edge  graphs  of  both  polyhedra,  although 
the  mapping  is  many-one.  We  are  given  a  generator  for  a  constraint  (qa,<Jh)- 
The  constraint  is  placed  in  ACF  boundary  condition.  This  requires  aligning  an 
(applicability)  edge  of  A  with  a  face  of  B  (or  vice  versa).  (This  point  is  fundamental 
to  understanding  the  correctness  argument:  if  both  contact  vertex  and  applicability 
vertex  must  lie  on  the  applicability  plane,  then  the  applicability  edge,  which  is 
an  actual  edge  of  A,  must  be  aligned  with  the  plane).  We  are  then  asked  to  find 
all  constraints  which  can  be  simultaneously  placed  in  ACF  boundary  condition. 
This  is  equivalent  to  asking,  “Given  one  edge  of  A  aligned  with  some  face  of 
B,  and  maintaining  this  alignment,  what  additional  edges  can  simultaneously  be 
aligned  with  faces  of  B,  such  that  all  associated  constraints  are  in  ACF  boundary 
condition?”  (By  associated  constraints  we  mean  the  following:  the  aligned  edge  is 
considered  as  an  applicability  edge.  Since  the  applicability  edges  cover  the  edges 
of  the  polyhedra,  the  associated  constraints  for  an  applicability  edge  eA  are  those 
C-functions  for  which  the  orientation  of  uetor mines  applicability.) 

Now,  by  fixing  an  edge  e/v  at  some  arbitrary  aligned  orientation  (with  a 
face  of  B),  we  retain  one  rotational  degree  of  freedom  p  about  eA.  We  wish  to 
choose  this  rotation  such  that  (1)  the  constraint  associated  with  eA  (i.e.,  (gA,  g/j)) 
remains  applicable  (and  of  course,  in  boundary  condition),  and  (2)  a  maximal  set 
of  constraints  is  simultaneously  placed  in  boundary  condition.  The  replacement  set 
we  compute  is  the  union  of  these  maximal  sets.  (2)  requires  a  maximal  number  of 
additional  edge  alignments,  and  must  also  preserve  the  disjoint  interior  criterion. 
So  choosing  p  so  as  to  maximize  the  number  of  edge  alignments  propagates  the 
alignment  constraint.  Clearly,  by  propagating  the  alignment  constraint,  we  obtain 
a  set  of  coplanar  edges  (recall  that  A  and  B  are  convex).  Fach  edge  represents  a 
contact  vertex  and  an  applicability  vertex  for  (one  or  more)  /\CFs  in  applicability 
boundary  condition.  The  associated  generators  must  also  bo  coplanar, 

Wc  have  seen  that  all  replacement  generators  r^)  must  be  coplanar  with 
gA.  (As  usual,  there  exists  a  symmetric  argument  Tor  r(r 7/;)).  Given  an  expiring 
generator  g/>  on  a  polyhedron  V,  we  wish  to  predict  replacement  sets.  Replacement 
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sets  are  obtained  from  maxima!  sets  of  coplanar  generators  which  contain  vertices 
of  gp,  Clearly,  the  maximal  coplanar  sets  of  generators  for  a  convex  polyhedron 
are  exactly  its  faces  (and  their  boundaries).  Hence,  to  predict  replacement  sets,  we 
must  find  the  set  of  faces  of  P  (along  with  their  boundaries)  that  contain  vertices 
of  gp.  This  set  is 

${gp)  —  St(t;erf(0/>),  dP).  | 

5.7.  Analysis  and  Evaluation 

We  have  implemented  algorithms  for  moving  in  some  selected  rotational 
direction  until  cither  the  goal  or  a  C-surface  is  reached:  The  naive  algorithm  (see 
section  5.3),  the  predictive  update  algorithm  based  on  the  C-surface  family  as  a 
loose  maximal  bound  on  the  replacement,  set  (section  5.6),  and  the  incremental 
update  algorithm  based  on  Q  (section  5.6.3).  Wc  next  show  that  the  naive  algorithm 
and  the  Q  algorithm  both  have  the  same  asymptotic  complexity.  This  incans  that 
their  performance  will  largely  depend  on  the  constant  factors  in  the  computation. 
We  discuss  empirical  results  to  indicate  the  size  of  these  constants. 

5.7.1.  Complexity 
Naive  Algorithm:  O(nlogn) 

Let  N  be  the  number  of  C-surfaces  in  the  environment  (including  non- applicable 
C-surfaccs).  If  the  moving  object  is  made  up  of  mo  convex  polyhedra  with  ko 
generators  each,  and  the  obstacle  environment  comprises  Tin  convex  polyhedra 
with  jo  generators  each,  then  clearly  N  =  jokymono.  The  complexity  of  the  naive 
algorithm  is  as  follows: 

(i)  Intersect  trajectory  with  all  C-surfaccs  ( 0(N )). 

(ii)  Sort  intersections  around  SK  ( 0{N  logiV)). 

(iii)  For  each  intersection,  determine  if  it  is  applicable  and  on  the  boundary  of  a 
C-Space  obstacle.  First,  test  to  sec  if  the  ('-surface  is  applicable  by  examining 
its  ACFs.  If  so,  there  are  two  options:  ( I )  if  the  applicability  set  is  known  at  the 
intersection  point,  wc  can  test  to  see  if  the  other  C- functions  in  the  family  are 
negative  or  zero.  (2)  If  the  applicability  set  is  not  known  at  the  intersection  point, 
wo  can  compute  the  displaced  face  of  the  Minkowski  solid  corresponding  t<  the 
two  generators  for  the  ('-surface.  Next,  test  to  sec  whether  the  intersection  point 
falls  within  the  face,  (l)  would  make  this  step  O((.7oko)"morlo)  —  0(jok\)N). 


However,  (2)  needs  only  examine  the  generators  of  a  constraint,  and  allows  this 
step  to  be  0{N).  ( 0(N )). 

We  see  that  the  complexity  of  the  naive  algorithm  is  O(AMogfV). 

Q  Update  Algorithm:  0[N  log  N) 

The  complexity  of  the  Q  update  algorithm  is  as  follows: 

(i)  Intersect  trajectory  with  all  applicable  C-surfaces  and  iheit  ACFs.  Let  the 
number  of  applicable  C-surfaces  be  y  <  N,  and  the  number  of  ACFs  per 
C-surfacc  be  a.  (y(l  +  a)  intersections  =  O(y)). 

(ii)  Sort  the  intersections  around  Sl.  (0(^  log  ^ ) ). 

(iii)  For  each  intersection:  Sort,  add,  and  delete  j  C-surfaces  from  the  intersection 
queue.  (0{j  logy)). 

This  yields  complexity: 

N  N  ^  N  N 

~k  log  1 1  +  ;  Iog;  “  “fc(log  N  ~  log  *)  +  log^ 

=  0(N  log  N). 

In  the  next  section,  we  justify  treating  k  as  a  constant. 

Actual  Performance 

In  practice,  the  naive  algorithm  has  run  faster  than  the  Q  update  algorithm 
for  the  specific  problem  of  rotating  until  a  C-surfacc  (or  the  goal)  is  reached.  As  the 
complexity  analysis  has  shown,  since  both  algorithms  arc  0{N  log  TV),  the  difference 
in  performance  will  be  due  to  different  constant  factors.  A  good  estimate  for  k  is 
10.  For  example,  in  a  typical  environment  with  624  type  (a),  704  type  (b),  and  J 872 
type  (c)  C-surfaces,  4  sample  applicability  sets  have  sizes  353,  362,  365,  and  355.  j 
is  quite  small;  for  this  environment  it  is  typically  between  2  and  40.  The  number  of 
ACFs  per  C-surfacc  depends  on  the  degree  of  the  vertices.  For  trihedral  vertices, 
for  example,  a  <  4  (type  (c)  C-surfaces  have  4  ACFs).  Hence  this  tends  to  balance 
out  any  possible  gains,  since  /c  is  not  much  bigger  than  a. 

Once  more  we  should  remember  that  the  §  update  algorithm  is  designed  to 
solve  the  more  general  problem  of  applicability  decomposition  of  SO( 3),  while  the 
naive  algorithm  has  been  specialized  to  solve  the  “rotate  to  a  C-surfacc”  problem. 
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Wc  speculate  that  similar  specialized  algorithms  may  be  developed  as  fast  solutions 
to  specific  spatial  planning  problems.  However,  decomposition  tools  arc  a  more 
general  solution  which  can  be  applied  to  a  whole  class  of  spatial  planning  problems. 

5.7.2.  Related  Work,  Searching  and  Lazy  Evaluation 

The  implemented  planning  system  is  described  in  chapter  2.  The  control 
structure  of  the  algorithm  is  a  search.  The  search  employs  certain  local  operators 
for  moving  between  configurations.  One  such  local  operator  is  precisely  the  “rotate 
to  a  C-surface  (or  the  goal)’’  algorithm. 

As  for  most  heuristic2  search  algorithms,  an  adversary  can  probably  devise  a 
find-path  problem  which  must  require  an  exponential  amount  of  time  to  solve.  This 
does  not  imply  that  a  polynomial-time  algorithm  using  the  mathematics  presented 
in  this  thesis  could  not  fce  devised;  indeed,  the  theoretical  work  of  Schwartz 
and  Sharir  (1982a)  suggests  this  possibility.  However,  in  practice,  the  planner  has 
performed  quite  well.  We  offer  the  following  explanation  for  why  the  planner  should, 
in  average  cases,  perform  better  than  in  the  adversary  situation. 

In  the  theoretical  work  of  Schwartz  and  Sharir  (1982a)  and  Schwartz  and 
Sharir  (1981),  the  concept  of  non-critical  regions  is  introduced.  A  non-critical 
region,  intuitively  speaking,  is  a  region  in  (free)  configuration  space  where  the 
constraints  are  invariant.  We  employ  similar  constructs  via  sets  of  non-redundant 
constraints,  and  by  means  of  applicability  sets.  In  Schwartz  .and  Sharir  (1981),  for 
example,  free  space  is  decomposed  into  critical  and  non  critical  regions,  and  the 
connectivity  of  these  regions  computed.  The  connectivity  graph  is  then  searched 
for  a  path.  However,  computing  these  regions  is  (geometrically  and  algebraically) 
quite  difficult.  The  regions  are  at  least  as  complex  as  the  C-Spacc  obstacle  and 
applicability  regions. 

Instead  of  precomputing  the  applicability  regions  (or  knowing  them  a  priori ), 
our  planner  computes  them  as  it  explores  configuration  space.  While  in  the  worst 
case  the  entire  applicability  decomposition  must  be  calculated,  this  case  does  not 

2Wc  use  tin’  term  heuristic  in  reference  to  the  time  complexity,  mid  not  the  completeness  of  the 
algorithm. 
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arise  in  practice.  We  effectively  adopt  a  policy  of  lazy  evaluation  of  applicability  in 
devising  the  planning  algorithm. 


6 

The  C-Voronoi  Diagram  and  its  Relationship 

to  Intersection  Manifolds 


6.1.  Introduction 

Tor  a  finite  set  of  points  P  in  the  plane,  the  Voronoi  diagram  is  the  set  of  all 
points  in  the  plane  which  are  equidistant  from  two  or  more  points  in  P.  The  Voronoi 
diagram  for  P  is  a  network  of  straight  line  segments.  Drysdalc  (1983)  introduced 
the  generalized  Voronoi  Diagram  (or  GVD)  for  the  plane:  for  a  set  of  polygons  in  the 
plane,  the  GVD  is  defined  to  be  all  points  in  the  plane  which  lie  (perpendicularly) 
equidistant  between  two  or  more  polygons.  The  GVD  is  a  network  of  straight  line 
segments  and  parabolic  sections.  If  the  polygons  are  considered  as  obstacles,  the 
GVD  represents  the  network  of  paths  through  frce-space  which  maximize  clearance 
from  the  obstacles.  Brooks  (1983a)  and  O’Diinlaing  and  Yap  (1983),  O’Dunlaing, 
Sliarir  and  Yap  (1982)  have  developed  definitions  and  algorithms  employing  an 
extension  of  the  Voronoi  diagram  for  low-dimensional  configuration  spaces.  Nguyen 
(1983)  also  discusses  the  relationship  of  global  methods  to  the  GVD. 

More  formally,  the  generalized  Voronoi  diagram  (and  its  extensions)  decompose 
the  free  space  into  a  set  of  regions,  {  iif, },  such  that  all  points  X  G  Hi  are  closer  to 
one  obstacle  than  to  any  other.  Thus  points  on  the  GVD  are  equidistant  from  two 
or  more  obstacles. 


Figure  6.1.  A  picture  of  the  generalised  Voronoi  Diagram  for  a  bounded  2D  workspace  containing 
four  polygonal  obstacles.  Reprinted  with  permission  from  Nguyen  (1983). 

In  this  chapter,  we  extend  the  concept  of  the  generalized  Voronoi  diagram  to  the 
six  dimensional  C-Space  3?n  X  50(3),  to  provide  a  formal,  constructive  definition 
of  the  C- Voronoi  Diagram,  or  CVD.  The  CVD  is  an  attractive  construction,  in  that 
it  contains  a  representative  component  for  each  “branch”  of  free  space.  Each  such 
component  is  submanifold  of  dimension  0  <  d  <  5,  called  a  Voronoi  manifold.  We 
will  derive  the  following  connection  between  intersection  manifolds  and  the  CVD: 

Let  p  be  a  path  along  the  CVD.  p  lies  along  a  connected  chain  of  Voronoi 
manifolds,  "Vj , . . . ,  "V*.  We  demonstrate  that  for  each  Voronoi  manifold  "V* ,  there 
exists  an  equivalent  intersection  manifold  of  level  C-surfaccs,  i,.  Furthermore, 
we  also  show  that  for  every  connected  chain  or  Voronoi  manifolds,  there  is  an 
equivalent  connected  chain  of  intersection  manifolds  (of  level  C-surfaces).  (The 
equivalence  we  demonstrate  is  actually  stronger  that  homotcpic  equivalence). 

This  yields  an  immediate  (theoretical)  completeness  result  for  planning  along 


intersection  manifolds.  While  our  proof  is  constructive,  it  cannot  be  considered  an 
effective  procedure.  The  charts  for  the  Voronoi  manifolds  arc  undoubtedly  very 
diflicult  to  derive.  This  in  turn  makes  it  hard  to  develop  planning  algorithms  along 
the  C- Voronoi  diagram.  In  C- Spare,  the  most  attractive  feature  of  the  CVD  is  not 
that  it  maximizes  clearance  from  obstacles,  but  that  it  represents  the  connectivity 
of  free  space.  In  other  words,  given  the  CVD,  the  Movers’  problem  can  be  solved 
by  connecting  the  start  and  goal  configurations  to  the  same  connected  component 
of  the  CVD.  Dut  since  the  Movers’  problem  has  already  been  reduced  to  the  task  of 
navigating  a  point,  it  is  clear  that,  modulo  some  uncertainty  bound,  wc  do  not  ,'.ced 
to  maximize  clearances  while  in  planning  paths  in  C- Space.  We  demonstrate  that 
instead,  it  is  possible,  in  principle,  to  devise  a  planning  algorithm  along  intersection 
manifolds  -for  which  wc  have  derived  charts  (chapter  4) — which  is  equivalent  to  a 
planner  along  the  CVD. 

Generalized  Voronoi  Manifolds 

In  this  section  wc  define  the  C-  Voronoi  Diagram  [CVD)  for  the  configuration 
space  D?3  X  50(3).  Note  that  for  Euclidean  configuration  spaces  we  would  employ 
the  standard  techniques  (Drysdale  (1983)).  The  metric  in  9i3  X  50(3)  is  non-obvious, 
and  the  CVD  docs  not  reduce  to  the  CVD  when  rotations  are  factored  out.  However, 
it  has  the  same  connectivity  as  the  CVD.  The  CV  D  Tor  configuration  spaces  without 
a  Euclidean  distance  metric  is  fundamentally  different,  and  is  defined  as  follows. 

To  define  the  CVD,  we  rely  on  the  collection  of  pseudo-metrics  provided  by 
the  geometric  interpretation  of  C-function  values  (chapter  3).  Intuitively,  within 
some  well-defined  region  in  frce-space  where  a  C-function  is  non-redundant,  its 
value  characterizes  the  translational  distance  to  cither  (l)  an  obstacle  face,  or  (2) 
the  plane  of  the  obstacle  face.  Formally: 

In  this  chapter,  wc  will  use  7  C  0J3  X  50(3)  to  denote  free  space.  See  chapter 
2  for  a  formal  review  of  charts  and  atlases.  As  noted  in  more  detail  in  chapter  2,  in 
this  thesis  we  usually  specify  charts  via  the  inverse  form  h  :  En  — *•  M  (where  En  is 
an  open  subset  of  5JT*)  with  the  understanding  that  it  is  the  inverse  (or  set  of  local 
inverses)  /i  ~*  which  provides  the  family  of  charts  { (h  ~l,Wi)  },  for  U,  W,  —  h(En). 


Definition  (1):  Let  At  be  the  set  of  families  of  C-functions  on  Di,f  X  S '0(3).  I'or 
X  £  7,  let  Ax  be  the  set  of  maximum,  applicable,  non-redundant  C-functions 
within  families,  that  is,  if  M  £  M  is  a  family  of  C-functions,  and  A f  C  M  is 
the  subset  of  applicable  and  non-redundant  C-functions  at  X ,  then  M  contributes 
to  Ax  the  function  /  £  M'  such  that  /(A)  >  /i(A)  for  all  h  £  M 1  —  {  /  }.  If  k 
functions  in  M'  tie  for  maximum,  then  M  contributes  all  k  to  Ax- 

Let  «  be  the  dimension  of  C-Space.  Now,  X  £  CV D  if  there  exists  a  maximal 
subset  II  of  ,  containing  at  least  two  and  no  more  than  n  C-functions,  such  that 
all  functions  in  B  have  the  same  value  6y  at  X  and  all  functions  in  Ax  ~  B  have  a 
value  greater  than  to-  We  say  that  X  £  CVD  lies  on  an  (n  —  \B\  +  l)-dimensional 
Voronoi  Manifold.  The  C-Voronoi  Diagram  for  C-Space  is  the  union  of  these 
Voronoi  Manifolds. 

We  have  seen  that  a  level  C-manifold  is  analogous  to  a  level  surface  in  Oi3,  in 
that  it  is  the  set  of  configurations  {A  |  /(A)  =  t }  for  some  applicable  C-fuiiction 
/.  Clearly,  points  on  a  /c-dimensional  Voronoi  manifold  V  lie  on  the  intersection  of 
n  -  Cl  1  equal  level  C-manifolds,  i.c., 


/iW  = 


where  the  level  ^(A)  is  allowed  to  vary  as  X  moves  along  V .  Furthermore,  we  insist 
that  the  C-functions  /,•  constructing  the  Voronoi  manifold  must  belong  to  pairwise 
distinct  families. 

When  we  say  that  a  Voronoi  manifold  "V,  is  constructed  from  a  set  of  constraints 
,  we  mean  that  all  the  C-functions  /  £  l'\  have  equal  value  along  V,.  An  intersection 
manifold  constructed  from  F,  is  the  intersection  of  level  C-surfaces  for  constraints 
in  l'\.  By  this  we  mean  that  first  a  level  is  chosen  for  each  /  £  Ft,  and  then 
the  resulting  level  C-surfaces  arc  intersected.  In  general,  a  level  C-surface  for  a 
C-function  /  at  level  i  has  the  form 


Figure  6.2.  Strongly  equivalent  intersection  manifold's,  and  the  bridge  manifolds  connecting  them. 
Each  "V,  is  strongly  equivalent  to  Each  bridge  manifold  /?,,,+ 1  is  equivalent  (but  not  strongly 
equivalent)  to  "Vj. 

/“'*(/)  is  the  trivial  intersection  manifold,  that  is,  the  manifold  constructed  by 
intersecting  one  level  C-surface  with  itself.  The  intersection  of  two  level  C-surfaces 
is 

j;- /-'(ton  »■'«*). 

The  intersection  manifold  J[  is  constructed  from  the  same  C-functions  at  different 
levels: 

An  Overview  of  the  Proofs 
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Let  p  be  a  path  along  the  CV D.  p  lies  on  a  connected  chain  or  Voronoi 
manifolds.  Call  this  chain  V| , . . .  ,  V*.  For  a  Voronoi  manifold  V,  we  say  that  an 
intersection  manifold  J,  is  equivalent  to  V,  if  (1)  the  set  of  C  functions  which 
construct  "V,  is  a  (possibly  non-strict)  superset  of  the  (-functions  F[  which  define 
J,,  (2)  J;  is  hoinotopically  equivalent  to  "V,-,  (3)  J,  lies  in  tree  space,  and  (4)  .all 
C-functions  in  F[  satisfy  definition  (l)  along  J,.  If  l'\  =  i.e.,  J,  is  constructed 

with  exactly  the  same  C-functions,  then  J,  is  slronnly  equivalent  to  V,. 

Note  that  by  definition,  each  V,-  is  rest  ricted  to  where  the  conditions  of  definition 
(l)  hold,  i.e.,  to  some  region  NR(1'\)  C  7  where  all  C-functions  in  F,  constructing 
V,  arc  maximum,  applicable,  non-redundant  C-functions  within  families.  Now,  in 
general,  I,-  is  an  unbounded  level  set  which  cannot  lie  in  free  space  everywhere. 
Thus  by  convention,  we  also  restrict  J,  to  the  region  of  interest  NJt(l'l)  where 
all  C-functions  in  l1'-  satisfy  definition  (1).  That  is,  NR(F^)  { X  j  F\-  <_  }. 

Instead  of  writing  I;  fi  NR(Ffi  everywhere,  this  convention  is  assumed  throughout. 

In  is  an  interesting  question  whether,  for  every  Voronoi  chain  Vi, . . . ,  14,  there 
exists  a  connected,  finite,  corresponding  equivalent  or  strongly  equivalent  chain  of 
intersection  manifolds  I\,  ...,  J*,.'  (where  k  is  not  necessarily  equal  to  k1).  Theorem 

(I)  shows  that  fur  all  Voronoi  manifolds  V,,  there  exists  a  strongly  equivalent 
intersection  manifold  J,-.  These  J,-  might  not  form  a  connected  chain.  1  hcorems 

(II)  and  (III)  show  that  each  disconnected  pair  of  intersection  manifolds  J,  and 
Jj+ 1  can  be  connected  by  an  infinite  sequence  of  “bridges.”  Each  bridge  is  an 
intersection  manifold  equivalent  (but  not  strongly  equivalent)  to  "V,.  VVe  then  argue 
that  since  there  exists  an  infinite  bridge  sequence,  therefore  there  must  also  exist 
a  finite  bridge  sequence.  Finally,  (theorem  IV')  wc  show  that  there  exists  an  entire 
intersection  chain 


Theorem  I:  Let  V*  be  an  m-dimcnsional  Voronoi  manifold,  constructed  Ironi  a 
set  of  applicable,  non-redundant  constraints  A,  which  satisfy  definition  (1)  along 
yk.  Then  if  there  exists  an  intersection  manifold  1 *  of  level  C-surfaces  for  the 
constraints  A,  and  if  the  constraints  A  satisfy  definition  (1)  along  Ik,  then  14  an<l 
Jk  arc  hoinotopically  equivalent. 

Proof:  We  will  show  that  two  intersection  manifolds  constructed  from  the  same 
C-functions  at  different  levels  arc  hoinotopically  equivalent.  Next,  we  demonstrate 
that  the  Voronoi  manifold  is  essentially  a  special  case  of  intersection  manifold. 

Let  Vjt  be  an  tn-dirnensional  Voronoi  manifold, 


=  | /»(*)  =  -  =/«(*)}> 


(n  =  6  —  m  +  1) 


where  the  /,  are  chosen  from  A  as  defined  above  (see  definition  (1)).  Note  that 
the  value  of  the  /,  may  vary  with  X  €  14-  Let  Ik  be  a  (m  —  1)  dimensional  (or 
«  i *  .  _  i  :r  __  n\  r*  » itv  o  n  i  r.r  louni  n.QnrfnrpM  constructed  from 

U'UlUlt'UhiUlldl,  11  •  •  *  -  J  mvviv'.wviVMi  a.*******  v-v»  —  - - - - -  ^ 

the  same  functions  /,•: 


Ik  =  {  X  |  fX{X)  -  lx,h{X)  =  1-2,  .  .  .  ,  fn(X)  =  In  >• 

The  region  of  interest  for  V*  and  Ik  is  of  course  restricted  to  N H[{  /,  }),  where  the 
functions  {/,  }  satisfy  definition  (l).  Ik  dilfers  from  Vk  in  that  on  V*  the  values 
(levels)  of  the  functions  /,  are  equal,  whereas  on  Ik,  they  are  not.  Furthermore,  on 
\'k  the  value  varies,  whereas  on  J.k  the  values  are  fixed. 

Ik  may  be  expressed  as 


i 

The  claim  is  that  Ik  is  hoinotopically  equivalent  to  14,  that  is,  that  if  o  :  Iik  — *•  J 
is  a  chart  for  Ih  and  g'  :  Ek  -*  7  is  a  chart  for  14,  then  there  exists  a  continuous 
hoinotopy  deformation  h  :  Ek  X  f1  -*•  7  between  g  and  g'  sucli  that 
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h(Y,0)  =  g(Y) 
h(Y,  1)  =  g'{Y). 


As  usual,  /'  denotes  the  unit  interval  [0,  1).  For  a  review  of  elementary 
homotopy  theory  consult  appendix  II. 


The  charts  g  and  g'  exist,  since  J*  and  “14  are  manifolds.  (Assume  without  loss 
of  generality  that  only  one  chart  is  required).  A  level  C-manifold  /,• 1  (f)  (for  some 
level  l)  is  a  5-dimensional  manifold  and  hence  there  exists  a  chart  Ct  :  Z£5  — *•  7  for 
/t“'(f).  We  demonstrate  such  charts  in  the  proof  of  claim  (1. 1),  below. 

Let  £\  be  any  achievable  value  for  the  functions  /,  along  the  Voronoi  manifold 
"V&,  that  is,  any  ly  such  that  there  exists  some  X  £  ~Vk  satisfying  definition  (l)  Tor 
which 

/iPO-...  =  fn{X)  —  ly. 


Now,  r/j  +  ^  t 


.  \  r 

cjrv 


18  a  linear  nmimnHiion  cn 


i.ue  levels 


ft _ 1  /U 

'i  auu  r  y 


c  _ 
iui 


r 

Ji • 


o : _ _ l_ 

OU1UC  Vii'-II 


level  C-surface 


/r‘(«i  +  a  -  o<v) 

is  a  manifold,  each  has  a  chart  of  the  form  Ct)  above.  If  t  E  [0, 1],  these  are 
C-surfaccs  for  /;  with  level  /  G  ^v]j  an<i  tl icir  charts  may  be  parameterized1 
by  t.  Suppose  we  have  a  set  of  several  level  C-surfaces  (as  in  J^).  Their  charts 
may  be  intersected  to  form  a  new  chart  for  the  intersection  manifold.  Wc  define 
h  :  X  /'  — +  7  to  be  the  chart  for  the  intersection  manifold  at  c,  such  that 

We  call  h  a  charl  family  for  the  intersection  manifold. 

Claim  (I.  l)\  The  chart  family  h  can  be  constructed  such  that  h{Y  f  t )  is  continuous 
in  Y  and  t,  within  the  area  of  inte  rest  for  "V*  and  !*•  (For  proof,  see  below). 

'See  tlir  proof  of  claim  (1.1),  whore  /i(,-  is  such  a  chart. 
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Recall  that  is  the  value  (or  level)  of  /,■  on  the  intersection  manifold  I*.  Then 
h  :  Ek  X,  I1  7  is  a  homotopy  between  g  and  g',  that  is,  h  continuously  deforms 
Ik  into  "Vfc(^y),  where  we  use  T4(£y)  C  14  to  denote  the  Voronoi  manifold  restricted 
to  level  ly,  i.e., 


Mh)  =  {x\fi{X)=*---  =  /„po  =  Mc  r. 


Verify  that 


» 

=  h, 

and  that 

h{Ek,Q  )  =  f)filUy) 

—  T4(£y). 

We  have  shown  that  J *  14(£y)  (~ h  denotes  homotopic  equivalence)  for  all 

achievable  £y.  We  must  now  show  that  T4(^y)  14(/v  +  <0- 

We  are  interested  in  continuous  deformation  within  7.  Hence  V *  may  be 
multiply  connected  within  7,  so  long  as  it  does  not  wrap  around  obstacles,  14  must 
be  contractible  to  a  point  (within  7).  This  is  guaranteed  by  the  construction  of  A 
(definition  (l))  and  14,  i.e.,  by  the  choice  and  domains  of  the  functions  To  sec 
this,  consider  that  if  14  did  wrap  around  an  obstacle  in  C-Space,  then  the  value  of 
some  /,  would  have  to  go  negative.  Hence,  it  would  become  redundant,  and  could 
not  be  used  in  the  construction  of  14-  Note  that  the  C-Voronoi  diagram,  which  is 
the  union  of  Voronoi  manifolds  such  as  14,  will,  in  general,  wrap  around  obstacles 
and  be  multiply  connected. 

Furthermore,  we  can  choose  c  such  that  the  topology  of  14  docs  not  change 
too  drastically  between  14(£y)  and  T4(£y  4-  c).  (This  is  possible  since  I4  is 
finite-branching).  So 
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Ik  —A  ”Vjt(£v)  —A  "Vi(^V  +  «l)  —A  ^*{^V  +  e2)  —A  •  •  •  —A  ”V*(^V  +  cr). 

What  we  have  shown  is  that  I ^  is  homolopically  equivalent  to  the  “easy”  parts 
of  14  (where  the  level  of  the  Voronoi  manifold  is  constant).  We  next  showed  that 
because  the  topology  of  is  simple,  we  can  paste  together  these  restricted  Voronoi 
manifolds.  g 

Proof  of  Claim  I.t:  The  existence  of  a  continuous  chart  family  for  the  intersection 
manifold  is  based  on  our  knowledge  that  the  manifolds  exist  at  certain  levels,  and 
from  our  ability  to  demonstrate  such  a  chart  for  the  intersection  manifold.  In 
chapter  4,  we  exhibited  C-funclions  of  six  variables  for  the  C-Space  SR3  X  SO(3): 


fi  ■  (*:!/.  “►  SR 

which  are  continuous,  affine  in  x,  y,  and  z,  and  multilinear  in  the  sines  and  cosines 
of  tne  angles  V'i  0,  and  <j).  The  Linear  Form  for  a  C-function  /,  :  9?*  X  SO( 3)  -*■  SR, 
is  an  equivalent  expression 


fi{x,  y,  z,  0)  =  i?i(0)x  +  E2{Q)y  +  /53(0)z  4-  /?4(0), 


where  Ej  :  SO(3)  — *•  SR  (for  j  —  1,  2,  3,  4).  Now, 


fi  ' (0  =  ^r^/(x,  y,  z,  ip,  0,  <p)  - 


■*VV\  M 


w 

l.  O 


(Wc  have  dropped  the  0,  since  the  functions  Ej  arc  constant  with  respect  to  x,  y, 
z,  and  £).  If  E\  =  0,  then  the  solution  for  y  or  z  may  be  employed.  This  yields  the 
obvious  chart 


C  :  E5  ->  Si3  X  50(3) 
(y,  z,  i>,  0,  <f>)  t-+ 


,  E*y  +  E3z  +  Ea~1  ,  a  j.\ 
( - - - ,y,  z,rl>,0,<t>) 


Ei 


which  wc  presented  in  chapter  4.  C  is  affine  in  l,  and  can  be  used  to  construct  a 
family  of  charts 


hv  :  (K,  *)  — >  K3  X  50(3) 


fVi* 


f  Vi  o  O  n» 


/r'(o  =  /<•'(«( +(!-<)<»)• 


which  is  continuous  in  Y  =  ( y ,  z,  ip,  0,  <p )  and  A  he  is  clearly  a  homotopy  between 
level  C-rnanifo!ds  for  /»•. 


Chanter  4  also  derives  charts  for  /,  '(A/u/y  \‘j),  A  '(A) Hi  lj\ij) fl  j\  ’(At), 
(and  so  on)  by  solving  the  C  functions  simultaneously  for  the  intersection  manifold. 
For  example,  a  chart  for  the  intersection  manifold  of  degree  three 


n  /."'(< i> 

■«W 

can  be  constructed  by  solving  three  .simultaneous  equations  with  the  form  of  (6.1). 
For  arbitrary  coefficients  Ej  and  levels  A>  this  intersection  may  riot  always  exist. 
However,  wc  know  o  priori  that  it  exists  for  the  specified  levels  A  and  l\ .  From 
the  form  of  (6.1),  it  is  clear  that  if  the  intersection  manifold  exists  for  some  levels 


ti  (and  if  the  coefficient  functions  Ej  arc  independent),  then  it  will  exist  for  all 
levels.  Wc  omit  a  discussion  of  intersection  manifolds  of  higher  degree:  the  reader 
is  referred  to  chapter  4  for  further  details.  R 

Corollary:  For  every  Voronoi  manifold  V,,  there  exists  a  strongly  equivalent 
intersection  manifold  I,-. 


Corollary :  If  is  the  set  of  constraints  used  to  construct  Vt,  let  N R(I'\)  denote 
the  region  in  frce-space  where  all  of  the  constraints  in  I'\  satisfy  definition  (1).  If 
X  exists,  then  in  every  connected  component  of  Nll(Fi),  there  exists  a  strongly 
equivalent  intersection  manifold  I;  built  out  of  F,-. 


Proof  of  corollaries:  All  f  £  Pi  exist  within  NR(P\).  Pick  any  X  £  NR(Fi). 
Evaluate  all  the  functions  in  l'\  at  X  to  obtain  a  sot  of  levels.  The  intersection 
manifold  must  exist  at  these  levels,  since  we  have  demonstrated  that  X  is  on  the 
intersection  manifold.  The  intersection  manifold  from  the  C-surfaces  at  these  levels 
is  by  definition  and  by  theorem  (I)  strongly  equivalent  to  X-  S 


Next,  observe  that  for  all  l  <  i  <  k,  cither  X  C  X+l  or  X+I  c  X.  In  other 
words,  to  move  from  X  to  X+i.  we  either  add  or  remove  one  or  more  constraints: 

Vi  =  {x|M*)= - A.) 

V«.,  =  {X|/,(X)  =  ...  =  /*,,,} 

and  cither  /cf-  >  i  or  /c,  <  1-  We  call  fcj  and  fc,  hi  the  degree  of  the  Voronoi 

manifolds. 


Wc  have  shown  that  for  a  Voronoi  chain  X,  •  •  • ,  Xfc,  a  sequence  of  intersection 
manifolds  2i , .  •  •  >  Jjfc  may  be  constructed  such  that  each  J,  is  strongly  equivalent 
to  X  (for  1  <  i  <  k).  However,  the  sequence  of  intersection  manifolds  may 
be  disconnected.  Wc  now  furnish  a  theorem  demonstrating  that  the  intersection 
manifolds  may  be  constructed  in  such  a  manner  that  they  ran  be  connected  together 
by  a  series  of  special  intersection  manifolds,  called  bridges. 

Theorem  11:  The  intersection  manifolds  I\,  1$, . . . ,  Ik  may  be  constructed  such 
that  each  pair  of  intersection  manifolds  2,  and  Jt  i  i  can  be  connected  by  a  sequence 
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of  “bridges.”  Each  bridge  is  an  intersection  manifold  equivalent  (but  not  strongly 
equivalent)  to  V,. 

Proof:  Let  I|  be  a  strongly  equivalent  intersection  manifold  to  "Vi,  constructed 
with  C-functions  F\.  Along  Ii,  all  constraints  in  Pi  are  non- redundant.  Let  T2 
be  the  next  Voronoi  manifold  after  "Vi  in  the  Voronoi  chain,  and  let  F2  be  the 
constraints  constructing  V2. 

Case  (if  Xf  F2  C  F\,  then  Vi  is  lower  in  dimension  that  T2.  We  can  construct 
!•>,  a  strongly  equivalent  intersection  manifold  to  "V21  which  is  councctcd  with  Ji, 
be  removing  one  or  more  constraints  in  F\,  (We  remove  exactly  the  constraints 
Fi  --  V 2).  This  is  possible  because  if  F2  C  Fi,  then  NR(Fy)  C  NR(F'i): 


n  n ■(»,) 

her 1 

h-  n  fj'i'i)-  <6-2) 

/(W» 

Strictly  speaking,  equation  (6.2)  should  employ  the  subset  notation  (C)  instead  of 
equality  (=),  since  Iy  and  I2  are  restricted  to  where  the  intersection  is  applicable  and 
non-redundant.  However,  the  equality  maxes  the  construction  more  transparent. 
Note  that  the  construction  still  works  with  the  subset  notation,  since  NR[Fi)  is  a 
subset  of  NR[F2).  Since  F2  C  F\,  I\  and  J2  agree  on  the  levels  for  C-functions  in 
F2.  Since  h  C  h,  h  and  J2  are  connected. 

Case  ( ii-a ):  Suppose,  however,  that  Fi  C  F2.  Then  V2  is  lower  in  dimension 
than  Vi,  and  NR(F2)  C  NR[F\). 

Wc  know  that  Jy  C  NR[Fi).  If  I\  fl  NR(F2)  7^  0,  then  we  can  construct  J2 
from  F2  such  that  J2  Cl  NR(F 2),  J2  C  Jii  and  in  addition,  J\  and  J2  agree  on  the 
levels  in  F\.  Construct  J2  as  follows:  pick  a  point  Xq  6  Ji  ("I  A  ii(F2).  Evaluate  each 
fj  £  F-2  —  Fi  at  Xo,  to  obtain  a  level  c.j  =  fj[X o).  Construct: 

1  -r» = ji  n(  n 

i 

I 

1 
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We  showed  earlier  that  J2  may  be  constructed  in  this  manner. 


Case  f ii-b ):  The  hard  case  is  when  [\NR[F<z)  —  0.  In  this  case,  we  must 
construct  some  6trongly  equivalent  (to  V2)  intersection  manifold  I2  C  NR{Fz)  with 
different  levels  from  I\  with  respect  to  the  C-functions  F\.  We  then  fcuisd  a  sequence 
of  bridge  manifolds,  entirely  within  NR[F\),  between  Ii  and  I2,  connecting  them 
together. 


The  bridge  intersection  manifolds  are  constructed  out  of  some  subset  Fji  C  Fi, 

and  each  bridge  manifold  is  equivalent  (but  not  strongly  equivalent)  to  "Vi .  The 

bridge  manifolds  are  formed  by  relaxing  one  or  more  constraints  in  Fj  to  be  able  j 

to  move  from  N Il(Fi)  —  NR{F2)  into  NR[F2)-  The  motions  slide  along  intersection  j 

manifolds  constructed  from  the  remaining  constraints.  Once  inside  NR{F2),  we  1 

1 

construct  J2  there,  and  I2  is  strongly  equivalent  to  "V2.  Note  the  levels  at  which  J2  I 

is  constructed,  (with  respect  to  the  constraints  in  Fj),  arc  typically  different  from  ' 

\ 

\ 

\ 
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Figure  6.4.  Case  where  Ji  f)  N Il(Ft)  —  8. 

the  levels  at  which  Ji  is  constructed.  The  existence  of  bridge  manifolds  is  covered 
in  theorem  III.  H 

We  will  use  the  dot  notation  for  the  Riemannian  inner  product  on  the  tangent 
space  at  X.  The  normal  to  a  level  C-surface  /'’(£)  is  V/,  and  depends  on  the 
inner  product.  We  assume  throughout  that  the  normals  are  unit  vectors.  A  basic 
concept  in  these  proofs  is  that  of  general  position,  or  transversality  (see  Hirsch, 
197G).  Two  submanifolds  M,  N  of  a  manifold  V  are  in  general  position  if  at  every 
point  of  Mf]N  the  tangent  spaces  of  M  and  N  span  that  of  V.  If  A  and  B  are 
not  in  general  position,  then  arbitrarily  small  perturbations  of  one  of  them  will  put 
them  in  general  position.  In  our  case,  M  and  N  correspond  to  level  C-surfaces  and 
their  intersection  manifolds,  and  V  to  5ft3  X  SO('i).  The  proofs  still  work  even  if 
M  and  N  are  in  general  position  only  at  “many”  points  of  M  f]N.  We  will  write 
the  condition  of  general  position  for  two  level  C-surfaces  /-1(^i)  and  g_,(£ 2)  as 

V/  Vff  ^  1. 


mm*. 


Theorem  lit  The  Existence  of  Bridge  Manifolds:  (Bridges  of  dimension  five). 
Let  S  be  a  path-connected  component  of  NR{Fi),  Xo,  Xi  €  S,  and  f,g€.  F\ .  Note 
that  5  lies  with  in  the  domain  of  /  and  g,  and  that  by  construction,  i(S)  is  an 
open  set.  Then  if  V/  •  Vg  i  within  S,  then  there  exists  an  infinite  sequence  of 
five  dimensional  bridge  manifolds  within  S,  connecting  Xo  and  X\ . 

Proof:  Each  bridge  manifold  will  be  of  the  form  f~\cj)  or  ff~’(ctf),  for  different 
levels  of  cj  and  c0.  Note  that  as  we  vary  cj,  f~i(c.j)  covers  5  (similarly  for  g). 
Let  Tx  denote  the  six  dimensional  tangent  space  at  X.  If  co  =  /(X),  the  level 
C-surface  /~'(co)  is  a  five  dimensional  submanifold  of  3J3  X  50(3),  with  a  five 
dimensional  tangent  space,  T That  is,  identifying  Tx  with  a  subspace  of  Tx, 

2^  =  {v€rx|o.V/(X)  =  0}. 

It  is  easy  to  show  that  U  T\  spans  Tx,  for  all  X  where  V /(X)  ■  Xg[X)  1. 

Let  Nx  denote  the  normal  space  at  X  with  respect  to  /,  such  that 

Nfx  =  {v€Tx\v=~aVf{X)}, 

for  all  scalars  a  5ft.  So  Tx  —  Nx  ®  Tx-  Clearly,  if  V/(X)  •  Vj(X)  ^  1,  then 
TV l  is  spanned  by  Tx  U  T ^ .  Hence  Tx  —  Tx  +  T'x’ 

Since  the  space  of  differentially  tangent  directions  to  the  two  level  C-surfaces 
at  X  is  equal  to  the  space  of  all  directions,  there  exists  an  infinite  sequence  of 
differential  moves  along  level  C-surfaces  foi  /  and  g,  at  different  levels,  to  realize 
any  path  within  S.  Since  5  is  connected,  there  exists  such  a  path  from  Xq  to  Xi. 

I 

Corollary  II LI:  (Bridges  of  dimension  four).  A  direct  result  is  the  existence 
of  a  sequence  of  bridges  which  are  four  dimensional  intersection  manifolds.  Let  /j, 
h,  h,  h  €  F\.  Suppose  that  within  5,  V/,(X)  •  V fj{X )  ^  1  (for  i  y).  Then 
there  exists  ar  infinite  sequence  of  bridges  between  Xo  and  Xj,  where  each  bridge 


Figure  6.5.  A  finite  path  iterating  along  levels  of  /  and  levels  of  g. 


w.))=/r'wn//'(<i)- 


('  7*  i) 


This  is  reasonable,  since  each  is  a  four  dimensional  manifold.  If  the 

normals  are  all  independent  at  Xt  then  the  direct  sum  of  the  tangent  spaces  to  all 
possible  intersection  manifolds  7y(i,j)  is  clearly  Tx' 


Tx = p-i'  n  r’i ) + n^)+-  +(T,ir\T,x)+-+(Tijc]T^)- 

(Of  course,  i  ^  j  for  all  terms  in  this  sum).  I 

Corollary  III. 2:  (Existence  of  a  finite  sequence  of  bridges).  Wc  now  argue  that  if 
there  exists  an  infinite  seq  icncc  of  bridges  from  Xo  to  X]  within  S,  then  there  also 
exists  a  finite  sequence. 
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Informally,  wc  argue  that  it  is  always  possible  to  to  move  a  certain  distance  c 
along  each  level  C-surface,  and  that  this  c  cannot  grow  arbitrarily  small.  First,  of 
all,  note  that  S  is  not  infinitesimal,  and  that  i(S)  is  an  open  set.  (If  it  were  not, 
it  might  be  necessary  to  make  an  infinite  number  of  differential  motions  to  remain 
within  S ). 

We  also  appeal  to  the  well-behaved  structure  of  the  level  C-surfac.es,  and  their 
intersection  manifolds.  The  level  C-surfaccs  are  smooth,  with  normals  that  change 
continuously.  (If  the  normals  changed  discontinuously,  wc  might  not  be  able  to 
take  finite  steps).  Thus  we  can  move  a  finite  (i.c.,  not  infinitcssiinal)  distance  along 
the  surfaces  to  a  point  where  the  normals  arc  still  independent,  and  where  the 
surfaces  are  “similar"  (i.c.,  having  normals  in  almost  the  same  direction  as  before). 
Furthermore,  for  any  two  levels  of  /  within  S,  there  exists  a  homotopy  between 
them..  These  continuity  arguments  indicate  that  it  should  be  possible  to  move  in 
finite  steps  along  the  intersection  manifolds,  and  hence  wc  can  reach  Xi  from  Xo 
ir.  a  finite  number  of  bridges. 

Suppose  from  Xo  to  X\  there  exists  an  infinite  sequence  of  bridge  manifolds, 
but  no  finite  sequence.  Then  either  (l)  %{S)  is  not  an  open  set  (and  therefore 
only  differential  motions  can  stay  within  it),  or  (2)  for  a  subset  P  C  S,  whose 
cardinality  is  that  of  the  continuum,  the  entire  tangent  space  is  not  available  along 
the  level  C-surfaccs.  In  both  cases,  our  initial  hypotheses  arc  violated,  (t)  violates 
the  assumption  i(S)  is  an  open  set,  and  (2)  the  assumption  of  general  position. 

We  formalize  this  argument  as  follows: 

Definition:  Let  U  bo  a  metric  space,  and  p,  p'  :  /'  — ♦  U  be  paths  in  U.  Let  {  Un  } 
be  an  open  cover  of  p(/’)  in  U,  where  each  (/„  is  a  neighborhood  of  radius  <  r, 
and  Uaf\p(P)  0.  We  say  that  p'  approximates  p  at  resolution  r  if  {  Un  }  is  an 
open  cover  for  p’(f')  also;  that  is,  if  p'[I])  C  U«  t4e- 

Claim  (III. 2.1)  shows  that  an  arbitrary  curve  in  some  neighborhood  U  of 
free-spacc  can  be  approximated  by  a  path  within  U  along  a  finite  sequence  of 
intersection  manifolds.  The  proof  of  Cor.  (III. 2)  then  employs  the  fact  that  the  curve 
is  compact,  and  therefore  can  be  covered  by  a  finite  number  of  such  neighborhoods. 
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Figure  6-6.  Let  e0  —  /(Xo)  and  —  f(Xt).  X0  lies  on  /~1(,'o)  f)  ff_,(co).  and  Xi  lies  on 
/-‘(c.inrVi)-  The  Path  segment  between  Xo  and  Xi  may  be  approximated  by  a  path  sliding 
first  -dong  g~l(co)  from  X0  to  X',  and  then  from  X'  to  Xi  along 

Claim  III. 2.1:  Any  path  within  a  neighborhood  where  /  and  g  are  defined,  may 
be  approximated  to  an  arbitrary  resolution  by  a  finite  sequence  of  motions  along 
level  C-surfaces  of  /  and  g. 

Proof:  We  will  regard  level  C-surfaces  of  /  and  g  as  trivial  intersection  manifolds. 
Consider  how  one  can  approximate  a  path  from  Xo  to  X\  by  a  path  along 
intersection  manifolds.  Let  d  be  a  metric  on  3?3  X  *50(3),  and  f,  g  be  C-functions 
with  inverse  images  covering  a  neighborhood  containing  Xo  and  Xj.  Let  co  =  /(Xo) 
and  ci  =  /(X t).  Xo  lies  on  the  intersection  manifold  /-1(co)  D  ?_1(ro)>  and  Xi  lies 
on  /_1(ci)n<?_l(cf)-  Construct  /-1(ci)  f]  Q~l[co)  (refer  to  fig.  6.6),  and  choose  X' 
to  be  the  any  of  the  closest  points  to  Xo  on  this  manifold.  We  can  construct  a  path 
which  slides  from  Xq  to  X'  along  g_l(c o),  and  then  slides  from  X'  to  Xi  along 
/-1(ci).  We  wish  to  demonstrate  that  by  choosing  Xi  sulficiently  close  to  Xq,  X1 
can  bo  made  to  lie  arbitrarily  close  to  Xq;  that  is,  for  all  e  >  0,  there  exists  a 


and  Cq'  ~ 


g (XQ) ,  Cl'  -  g(Xx). 

2H 


mi 

km 


k'?y'. 
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b  >  0  so  that  d(Xo,  X')  <  e  whenever  d(X otXi)  <  <5-  This  is  definitional,  since 
liraxj-^o  f~1(ci)  =  f~1(c o)- 

Thus  for  every  neighborhood  Bt(Xq)  of  radius  r  about  Xo,  there  exists  an  Xj  G 
J3r(Xo)  such  thatX'  G  J9r(Xo)  also.  Furthermore,  d(X q,X')  is  finite  (and  non  zero). 
Of  course,  a  similar  argument  holds  for  the  path  segment  between  Xi  and  X'.  Thus 
any  path  within  a  neighborhood  where  /  and  g  are  defined,  may  be  approximated  to 
an  arbitrary  resolution  by  a  finite  sequence  of  motions  along  intersection  manifolds. 


Clearly,  similar  arguments  hold  for  intersection  manifolds  of  higher  degree. 

Proof  ( Corollary  III. 2):  (Sketch)  Let  p(Il)  be  a  path  within  {(S')  from  Xq  to  Xi 
along  an  infinite  sequence  of  intersection  manifolds.  In  short,  p(Il )  C  f(S).  Choose 
an  open  cover  {Ua  },  relative  to  9?3  X  SO(3),  for  p(/1)  such  chat  (Ja  U&  C  f(S). 
p(/1)  is  compact,  hence  there  exists  a  finite  subcovcr,  i.e.,  for  finitely  many  indices 
ai, . . .,  an,  we  have 


Now,  for  each  Uai,  we  can  construct  a  path  p'{Il )  along  a  finite  sequence  of 
intersection  manifolds  approximating  p^Jpu/ai  (Claim  III.2.1).  That  is,  p'(Il)  is 
also  contained  within  the  closure  of  Ua..  Furthermore,  it  is  not  hard  to  construct 
p'  such  that  it  leaves  the  neighborhood  Uai  at  the  same  points  as  p,  that  is,  so 


a-V.a.  f)h  n  drr 

CJliib  J  |  \UUai 


/f  yh  A  orr  n  •  _  .  ji  •  •  #  ^  r  .  ?i  rr 

p  yi  )  |  \uua..  omce  bins  la  blue  lur  an  ua. , 


approximated  by  some  path  contained  within  Uai  U  •  •  •  U  UQn, 
some  finite  chain  of  intersection  manifolds.  I 


j  .  .  r  rl \  i 

t/iiuii  j  eaji  ue 
and  which  lies  on 


Theorem  (IV)  is  almost  immediate,  and  its  proof  similar  to  that  of  Theorem 

(I)- 

Theorem  IV:  For  every  connected  chain  of  Voronoi  manifolds  Cy  =  Vi, . . . ,  T*, 
there  exists  an  equivalent  (in  the  sense  of  theorems  (I — III))  connected  chain  of 
intersection  manifolds  Cj  =  such  that  the  entire  Voronoi  chain  Cy  is 

homotopicatly  equivalent  to  the  intersection  chain  Cj.  That  is, 
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Proof:  Simply  apply  the  fact  that  each  equivalent  pair  (V,/)  (where  “V  is  a 
Voronoi  manifold  and  J  is  an  equivalent  intersection  manifold)  must  lie  in  the  same 
non- redundancy  region.  Hence  if  one  chain  wraps  around  an  obstacle,  so  must  the 
other;  furthermore,  each  chain  must  wrap  around  the  same  obstacles.  R 

Future  Research 

There  are  several  interesting  problems  which  are  left  open.  They  include  the 
following: 

(i)  We  have  demonstrated  an  equivalent  chain  of  intersection  manifolds  for  any 
connected  chain  of  Voronoi  manifolds.  Show  -whether  or  not  a  strongly  equivalent 
chain  exists  also. 

(ii)  Show  whether  or  not  for  every  Voronoi  chain  C\  there  exists  a  (strongly) 
equivalent  intersection  chain  exhibiting  a  bijectivc  correspondence  to  Cy. 

(iii)  Devise  an  effective  procedure  for  constructing  a  chain  of  intersect  r.  manifolds 
to  realize  any  class  of  paths  in  free  space. 

(iv)  Derive  complexity  bounds  on  the  construction  of  the  C- Voronoi  diagram  and 
the  equivalent  intersection  chains. 

(v)  Other  configuration  spaces,  such  as  those  arising  in  the  hinged  body  problem, 
should  be  considered. 

(vi)  To  extend  these  results  to  configuration  spaces  generated  by  real  space 
constraints  which  are  not  polyhedral  (for  example,  algebraic  surfaces),  a 
generalization  of  the  GVD  such  as  smoothed  local  symmetries  (Brady,  1982b) 
could  be  considered. 


7 

Conclusion 


In  chapters  1  through  6,  we  developed  representations  and  algorithms  for 
automated  spatial  planning  with  six  degrees  of  freedom.  To  demonstrate  the 
competence  of  the  representation  and  the  feasibility  of  the  algorithms,  a  planning 
system  for  the  classical  tind-path  problem  with  six  degrees  of  freedom  was 
implemented.  The  planner  is  of  considerable  intrinsic  interest,  in  that  it  is  complete 
(for  a  given  resolution).  Experiments  have  demonstrated  that  this  algoiithm  can 
solve  find-path  problems  requiring  six  degree  of  freedom  solutions  that  were  beyond 
the  competence  of  earlier,  approximate  planners.  The  mathematical  framework 
developed  here  impacts  a  class  of  geometric  planning  problems  for  three  dimensional 
objects. 


The  planning  algorithm  may  be  explained  by  analogy  with  the  Point  Navigation 
Operators.  The  C-Space  transformation  reduced  the  motion  planning  problem  to 
the.  task  of  navigating  a  point  in  9N  X  50(3).  Since  the  path  for  the  point  must 
avoid  the  C-Space  obstacles,  which  are  curved,  six  dimensional  manifolds  with 
boundary,  clearly  paths  can  be  found  in  C-Space  by  the  closure  of  three  operators: 

(i)  slides  along  .1-  to  4-dimensional  intersections  of  level  C-surfaces; 

(ii)  slides  along  5-dimcnb'.onal  level  C-surfaccs; 

(iii)  jumps  between  6  dimensional  obstacles. 
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However,  these  simple  operators  could  not  be  implemented  until  a  series 
of  representational  and  algorithmic  questions  were  solved.  The  fundamental 
representational  issues  centered  on  the  structural  properties  of  the  defining  C- 
functions.  By  deriving  their  domains,  and  by  proving  theorems  about  the  topology 
of  these  domains,  it  was  then  possible  to  address  the  intersection  problem  for 
high-dimensiomd  configuration  spaces.  By  solving  these  open  problems,  developing 
representations  for  the  C-fuuctions  and  their  domains,  and  designing  decomposition 
algorithms  in  C-Spa.ce,  it  became  possible  to  represent  the  constraints  on  motion 
“completely,”  and  to  exploit  the  complete  representation  in  implementing  a  planning 
algorithm.  Next,  we  presented  new  theoretical  results  on  the  C-Voronoi  diagram. 
By  showing  that  for  every  connected  chain  of  Voronoi  manifolds,  there  exists 
an  equivalent  chain  of  intersection  manifolds  of  level  C-surfaces,  a  theoretical 
completeness  result  for  planning  along  the  intersection  manifolds  was  obtained. 
This  result  is  also  of  interest  since  while  the  charts  for  the  Voronoi  manifolds  are  as 
yet  unknown,  charts  for  the  intersection  manifolds  are  straightforward  (given  our 
representational  framework).  Thus  it  is  possible,  in  principle,  to  devise  a  planning 
algorithm  with  all  the  advantages  of  a  planner  along  the  CVD. 

There  is  much  work  to  be  done.  Ultimately,  decomposition  algorithms  such  aa 
those  we  present  in  chapter  5  will  become  increasingly  important  in  partitioning 
C-Space  into  regions  where  the  sot  of  applicable  (or  alternatively,  relevant) 
constraints  is  invariant  (see  also  Schwartz  and  Sharir  (1981)).  The  representations 
and  algorithms  we  have  developed  may  make  other  geometric  planning  problems — 
such  as  fine-motion,  and  planning  with  uncertainty — more  feasible,  and  should  now 
be  applied  in  these  applications.  The  find- path  algorithm  can  be  easily  extended 
to  robot  manipulators  with  six  degrees  of  freedom  in  which  translations  can  be 
decoupled  from  rotations.  This  class  includes  Cartesian  manipulators  (for  example, 
the  IBM  RS/l).  The  adaptation  of  this  work  to  a  production  environment  presents 
interesting  engineering  challenges. 

In  principle,  the  fiDOF  representations  can  be  extent  ,o  revolutc-joint,  linked 
arms  avitli  six  degrees  of  freedom.  However,  the  C-Space  of  the  linked-arm  problem 
is  the  six  dimensional  torus, 


5lX  ■■•XS1  (to  6) 


which  has  a  very  different  structure  from  3?3  X  50(3).  It  is  our  hope  chat  this  thesis 
can  present  a  methodology  for  formulating  the  geometric  constraints  for  arbitrary 
configuration  spaces,  and  that  a  similar  structure  will  be  found  for  constraints  on 
the  6-torua. 

Our  planning  algorithm  is  complete  (at  a  resolution),  in  that  the  representation 
employed  is  complete,  and  in  that  the  search  is  guaranteed  to  find  a  path  if  one 
exists  at  that  resolution.  However,  since  it  is  a  search  algorithm,  we  cannot  provide  a 
polynomial  time  bound.  Our  motivation  has  been  to  address  the  completeness  issue 
first,  by  resolving  fundamental  representational  questions;  now,  one  of  the  most 
important  remaining  tasks  is  to  develop  complete,  polynomial- time  algorithms  which 
can  actually  be  implemented.  We  do  not  believe  that  the  worst-case  exponential 
behavior  of  the  6D0F  planner  is  inherent  in  the  representation,  and  conjecture 
that  a  poiynomiai-time  algorithm  which  plans  pac’ns  along  intersection  manifolds 
can  be  devised.  Indeed,  the  theoretical  results  on  the  CVD  arc  suggestive  that  the 
limiting  complexity  of  the  approach  may  be  the  complexity  of  constructing  the 
CVD  or  an  equivalent  chain  of  intersection  manifolds.  More  research  is  needed  on 
the  topology  of  the  CVD.  A  fast  planning  system  might  determine  what  constraints 
construct  the  CVD,  and,  using  these  constraints,  construct  a  chain  of  intersection 
manifolds  which  could  attain  the  goal.  The  first  stop  in  this  effort  would  bound  the 
complexity  of  the  CVD  and  the  intersection  chains. 
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Appendix  I 


Details  of  the  Intersection  Problem,  and  Related  Problems 


This  appendix  contains  the  detailed  equations  from  chapter  4,  which  are 
relegated  to  an  appendix  because  of  their  excessive  length.  Some  definition  of  terms 
is  repeated,  so  that  the  interested  reader  will  not  have  to  flip  back  and  forth  too 
much. 


1.1.  Intersecting  Two  C-surfaccs  in  5R2  X  Sl 

By  applying  trigonometric  reductions  v/e  can  express  type  (a)  and  (b)  constraints 
as  follows  (only  (al)  and  (bl)  are  shown): 


cos(X,-)5p  +  C  bifi(Xj)y  —  sin(X;)£x  {  C  ccs(\,)x 
-f  sin(X;  -  iy)||6j||S  -  |ja;||  cos(Xt-  -  rjt) 

~  c  cos(x.'  -  7/)IIMI 


sin +  c,os{<f>j)x  -  J|a,||  sin (<£y  -  rjJS 

-  C||«i||  cos (4>j  -  T)i )  -  ||6y||  cos (tfy  -  7 /)  (bl) 

Where 

C  —  cos  0  and  5  =-•  sin  0. 

Now,  wc  can  consider  a  pair  of  these  equations  .as  a  system  in  four  variables, 
(x,y,C,S),  and  proceed  to  solve  (al)  and  (b2),  (bl)  and  (b‘2),  and  (al)  and  (bl)  for 


x  and  y.  For  example,  the  intersection  of  two  type  (a)  surfaces,  (al)  and  (a2)  is  a 
curve 

P  ■  r« i  f)r«2  -*■  JJi2  X  Sl 

where  rai  fira2  C  S'  denotes  the  intersected  applicability  constraints  for  (al)  and 
(a2).  Although  the  solutions  are  in  the  variables  C  and  S,  we  can  use  C  =  cos  0  — 
cos r  and  S  —  sin0  =  sinr  to  generate  the  curve  of  intersection  in  5ft2  X  Sl. 

After  much  simplification,  the  parametric  solutions  for  the  intersection  curves 
for  type  (a)  and  (b)  constraints  in  3J2  X  S1  are  as  follows: 

The  intersection  of  two  type  (a)  Surfaces:  (al)fi(a2) 
p0(r)  =  r, 

Px{r)  —  D^-(5(||a-||  cos{v!  +  Xt  ”  X.0  +  l!«/ll  009(7}  -  -  Xt  -  \-) 

—  ||a,||  cos(Xt-  +  7} i  -  X.)  —  j|a,||  cos(-Xt-  f  77,-  -  X*) 

+  C(2cos(X-  +  X;  -  7y)||6j||  -  2cos(X,'  4-  X;  -  7y)||by||)) 

+  C(l!«/ll  “  Xi)  ~  llarll  si niv'i  ~  \  ~  K) 

II  .  II  ..f\  I  ,  ^  \  ,  II-  II  \  I  1  \  ,\\ 

--  jjo-tll  t  </,  ~  *tj  ~r  ii^iK  t  'it  —  "1)/ 

+  C\ 2  sin(X?  4-  X<  -  7j)||6ji|  -  2  sin(X?  +  Xt  -  7j)IM) 

+  (-  sm(X-  +  X,  -  7J)  -  sin(X/  ~  X,  -  7y))ll&ji! 

+  (sin(X-  4-  X,  -  7j)  -  sin(X  ■  -  X,  +  7>))||fo>ll)/2), 

P„(r)  «=  l)^-(S(\\al\\ sinfa/  +  X,-  -  X/)  -  j|a/||  s\n(v-  -  X,-  -  X/) 

-  ija.lj  sin(X-  4-  7;  -  X.)  +  ||o,|j  s:n(-X  ■  +  7,-  -  X.) 

+  C{2  sin(X,-  4-  X;  -  7y)||fry||  -  2sin(X-  +  X,-  -  7y)iND) 

+  C(-i|of||  cos (n-  +  X,'  -  X')  -  |K|j  cos(t7  •  -  X,-  -  X D 
+  lh«H  cos (X-  +  7]i  -  X,)  4  ||a,i|  cos(-X-  4-  Vi  —  X,)) 
t  C\2  cos(X'  +  \i  -  7y)||6y||  -  2  cos(X-  4-  Xf  -  7y)lhjil) 

+  (cos(Xt-  +  X,-  -  7y)  -  cus(X,-  -  X,  -  7y))||f>y|| 

4-  (cos(Xt-  -  X,  4-  7j)  -  eo5(x»  +  X.  “  7y))|IM!)/2). 


where 


D  =  csc(Xt'  —  Xj). 


P 


P 


1 


1 


«.  vvr 
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The  Intersection  of  type  (a)  and  (b)  Surfaces:  (al)f](b2) 


where 


Mr) 

pAt) 


i 

F^S(‘2C||al-|j  cos[<f>j  —  r\[  —  X,-) 

+  \\bj\\  cos (<6y  +  X,-  -  7y)  -  ll&yll  cos(^-  +  X,-  -  7y) 

4-  llfcyll  cos(^y  -  X,-  -  7y)  4-  ||6y||  cos(4>j-  -  X,-  4-  7y)) 

+  |k?!!  sin(<£'  ~  k  +  X.)  -  2C2||a'||  sin(^.  -  k  ~  \) 
+  |k ||  sin(k  -  rj-  -  X,)  -  ||a,-||  sin(fj  +  *7,  -  X.) 

-  ||o,  ||  sin(k  -  Vi  +  x i ) 

4-  C(||bj||  sin(k  4-  Xt-  -  7j-)  -  ||6,-||  sin^j-  +  X.-  -  7y) 

-  ||6y||  sin (^J  -  X,-  -  7j)  -  ||by||  sin(^-  -  X,-  4-  7y))J, 


Pv(r)  =  -i^-S^Clk'll  sin(k  -  k  “  x») 

-  1 1 by 1 1  sin(k  -I-  X,-  -  7y)  +  ||by||  sin{«jiy  4-  X,  -  7y) 

4- 1 1 by 1 1  sin(k  “  \  -  7y)  -  INI  sin(k  ~  X*  +  7i)) 

4-  ||kll  cos(k  -  k  4-  X.)  4-  2C2||k||  cos(k-  -  k  "  *<) 

-  I!k||  cos(k  ~  Vi  -  \)  -  INI  cos(k  +  Vi  -  x<) 

-  ||at- 1|  cos(k  -  Vi  +  X;) 

4-  C'flkH  cos(0j  4-  X,  -  7J)  -  ||by||  cos(fy  +  Xv  -  7y) 

+  INI  “  xi  ~  7y)  ~  INI  C0SW  ~  X*  +  '*/)))» 


(2  cos(<j>j  —  \)S  —  2C  sin  [4>'j  —  \)) 
esc (0  —  <$>j  4  X,) 

2  ’ 
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The  Intersection  of  two  type  (b)  Surfaces:  (fel)f|(b2) 


Po(r)  —  r» 

px(r )  =  E^(S(||a/|j  cos(^j  +  tf>j  -  17  ■)  -  ||a,l|  cos(<£j-  +  <f>j  -  r) ,) 

-  ||a-||  cos(<£j  -  <f>j  -  r?  ■)  +  ||a,||  cos [<j>j  -  <f>}-  + »?,)) 

+  C(-||a-|| s\n(<f>'j  +  <j>j  -  r/-)  -f  ||aj|j  sin(<£y  +  <j>j  ~  Vi) 
+  ||at-||  sin(^j'  -  <f>j  -  i)-)  +  |k|j  sin(0]-  -  +  •?,)) 

-  ||fey||  sin(^  +  <t>j  -  7 j)  +  HMI  sin (<£'  +  4>j  -  7 j) 

+  !|fcj||  sin(fj  -  -  7y)  +  ||6y||  sin(^j  -  <j>j  -r  Ij))/*), 

pv(r)  =  E^(S(|ja,-|j  sin(<^  +  <l>j  -  rj-)  -  ||aj||  sin(^  +  <f>j  -  r/,) 

-r  ||at'||  sin{fj-  -  <j>j  -  17I)  +  ||a,|j  sin(^-  -  <t>j  +  Vi)) 

+  O’dla/H  cos{<£j  +  4>j  -  7j  ■)  -  ||a,||  cos(<6y  +  <f>j  -  r/,) 

+  ||a,-||  cos(<fey  -  <t>j  -  tfi)  -  ||a«||  cos(^j  -  <f>j  +  *j»)) 

+  ||&y||  cos(4>j  +  4>j  -  7 j)  -  llfcy!l  cos(<^  +  -  1 j) 

+  life'll  co8(*{  -  4>i  -  7y)  -  INI  <M+'j  "  *i  +  Try))/ 2), 

E  =  cac(^j  -  £,). 


1.2.  Related  Problems  in  S£2  X  Sl 


1.2.1.  Techniques  for  Moving  Along  C-Surfaces  in  9?2  X  Sl 

In  this  section  we  present  techniques  for  moving  along  a  C-Surface.  We  could 
imagine  using  these  methods  to  move  to  the  nearest  “edge”  (C-Surface  intersection), 
for  example.  A  level  C-Surfacc  is  defined  via  a  function  f(x,  y,  0}  ~  k  for  k  constant. 
/  is  exactly  of  form  (al)  or  (fel)  (above),  and  the  level  surface  in  3?2  X  is  all 
points 


L={Xedt2  Xrf\f(X)  =  k}, 
where  rj  C  Sx  is  the  0  applicability  range  for  /. 

Define  a  hypcrplane  in  9i2  X  Sy  as  the  set 

P  =  {  X  €  X  S{  |  X  ■  H  =  -ht  }, 

where  H  =  (hi,  hi,  h3 ). 

We  intersect  the  level  surface  L  with  the  hyperplane  P  to  obtain  the  intersection 
curve: 


PoW  =  r, 

Px(t)  —  G I  yS(cos(\i)(-~h3t  -  h4)  +  h2  sin(X,  -  7J)||fe/||) 

4-  C  sin(X,)(-ha<  -  h4)  -  cos(x»'  -  77,) 

-  Ch2  cos(X,  -  7y)||6y||  -  h2kj, 

py(t)  =  -Gjf5(sin(Xj(M  -f-  h.,)  +  hj  sin(X,’  -  7/)||6y||) 

+  C  cos(X,)(-/i3t  —  h/i)  -  hi  ||a,||  cos(X»  -  tji) 

~  Chi  cos (X,  -  7y)||tj||  -  hjfc).  (Ff|(al)) 


where 


G 


1  — 


_ 1 _ 

(h2  sin(X,)  +  hi  cos(X,))5  +  (/i|  sin(X,)  —  h2  cos(Xi))C 
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(pm)) 
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-G2[sin(<£y)(M  4-  hi)  +  /i2||«*||  sin(<£y  -  rji)S 
+  Ch2|ia*ll  cos(tf>j  —  rji) 

+  Mlfyll  cos(0y  -  7y)  f  h2fc), 

G2^cos(^y){/i3«  +  /i4)  +  hi|!a,||sin(^y  -  r?,)S 
+  C'/ii||a,||cos(^y  -*?,) 

+  hi\\bj\\  cos(^y  -  7y)  +  hikj, 


where 


G-2  = 


hi  sin(^y)  —  hi  COS (<£y)' 


1.2.2.  Characterizing  Clearance  to  a  C-Surface 


It  would  be  very  useful  to  characterize  the  minimum  clearance  to  a  C-surface. 
We  would  like  to  answer  the  question: 


*  For  a  point  bxy  <E  SJ'S/  at  what  orientation  is  bTy  closest  to  a  C- surface,  and 
what  is  minimum  directed  clearance  vector  at  that  orientation? 


Using  Lagrange  multipliers,  we  can  minimize  a  function  f[x,y,0 )  subject  to  a 
constraint  g(x,  y,  6)  —  0  by  constructing  the  auxiliary  function 


II(x,  y,  0,  l)  —  f{x,  y,  0 )  -  lg{x,  y,  0) 


and  solving  the  partial  derivatives 


£- 

f- 

S— 


dii  „ 

aT“° 


In  our  case,  g  will  define  a  C-surface,  for  example,  a  type  (a)  surface: 


g(xi  y,  0)  —  sin(0  +  X,)y  +  cos (0  +  X,)z  -  ||fcy||  cos {9  +  X,-  -  7 y)  -  ||«i||  cos(X,-  -  ??,•) 


4v!‘s>. 


0% 


Ai 


*73W'. 
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and  /  will  be  a  distance  function.  Now,  the  rotational  dimensions  cannot  be  treated 
uniformly  in  establishing  a  metric,  so  we  will  define  distance  in  Euclidean  space. 
Minimizing  the  square  of  the  translational  distance  suffices  for  our  purposes,  lienee, 

/(z,  y,  0)  =  (x  -  bx)2  +  [y  -  by)2. 

Differentiating  II  gives  us  the  following  equations: 

dH 


=  2(x  —  bz)  —  lcos(9  +  \j) 

=  2(y  —  fcy)  —  i  sin(0  +  Xj) 

=  ~^(cos(0  4-  \i)y  —  sin(0  +  X,)z  +  ||6y||  sin(0  +  X,-  —  qy)) 


dx 
dH 
dy 
dH 

39 
dll 

—  =  (-  sin(0  +  \)y  -  cos (0  +  X,)x  +  ||6y||  cos(0  +  X*  -  qy) 
+  lla.i|cos  (»?,  -X,)) 


(1) 

(2) 

(3) 

(4) 


Solving  these  equations  for  x,  y,  0,  and  t  is  not  trivial.  However,  the  following 
observations  make  it  easier.  First  of  all,  wo  note  that  solving 


at 


o 


(5) 


is  equivalent  to  solving  (4)  as  long  as  i  =  0  is  not  a  solution.  We  next  solve  (1)  and 
(2)  for  Ccos(0  4-  X,)  and  £sin(0  4-  X,)  and  substititute  this  value  in  to  (3)  and  (5). 
(5)  then  becomes  a  linear  equation  in  x  and  y  while  (3)  is  quadratic  in  x  and  y  and 
linear  in  t.  Our  rewriting  of  (4)  into  (5)  has  thus  eliminated  I  from  (5),  and  we  can 
solve  for  x  in  terms  of  y: 


-2y2  4-  (2||6y||  sin(qy)  +  2by)y  -  2x 2  +  (2||6y||  cos(qy)  4-  2 bx)x 

+  %»ll  «'os(t7,  -  X.)  -  26v||6yj|  sin(qy)  -  26^j|6>||  cos(qy)  (3) 

(2b.,  -  2||by||  cos(qy))y  4-  (2||6y||  sin(qy)  -  2 by)x 

-  2&.cj|6y||  sin(qy)  4  2&J|6y||  cos(-)y)  (5) 

We  need  one  additional  constraint:  this  is  obtained  by  observing  thr  t 

sin(0  4-  X,)2  +  cos{0  4-  \if  —  1  (6) 

Since  the  trigonometric  terms  can  be  expressed  in  x,  y,  and  f,  we  can  obtain  l2  in 
terms  of  x.  and  y.  (3),  (.r>),  and  (G)  then  result  in  a  quartie  in  x  with  the  roots: 


where 


x  =  Gi^26„||fry||2sin(27y)  +  26Ij|6y||2  cos(27y) 

-  ((2ftJ  +  2&2)||bj|j  +  2H6J8)  cosIt,-)  +  26,||6i||a 

±  ^(iNIIIMI  cos(tj,  -  X,  +  7y) 

+  IMIINI  C0S(Vi  -  K  -  7 >■)  ~  26x||ai|!  003(7,-  -  X,-))j, 


2(26»||6y|i  sin(7y)  +  26*||by||  cosfry)  -  ||fcy||2  -  6}  -  &2 ) 


and 

<?2  =  «“(7»)  ”  2MMI  coa(7y)  +  ||6y||2  +  62  +  *>2. 

Given  x,  y  is  found  from  (5)  : 


ni.'v  .11  c5nr.-v  v..  _  !!,\  I!  «;nf«v  -1  -1-  /.  I!/.  11  r««r-v  •'i 

ui^jii  "‘“V  U>  “vj~  -xn-,711 - V  IJJ  ■  -yil-jil - \'J/ 

INI  cos(7y)  -  K 


l  can  be  found  from  (3)  as  a  linear  function  of  1  and  y.  To  determine  0,  we  calculate 
sin(0  +  X,)  and  cos(0  •+-  X;)  from 


2(y  ~  <>; 

t 

coS(0  +  X,)  = 

and  use  a  two  argument  arctangent  function  Atan2  :  5?2  — ►  S1  to  determine  9  +  X;. 
The  0  value  must  be  checked  against  the  applicability  constraints  for  surface  <7; 
if  it  falls  outside  the  range,  then  endpoints  will  yield  the  minimum  clearance. 
Naturally,  it  is  possible  that  for  certain  orientations,  (bx,bv,0)  will  lie  on  or  inside 
the  C-surfacc.  These  cases  may  be  disambiguated  by  the  sign  of  g{bx,b u,0).  Finally, 
given  the  closest  point  (at  some  orientation  0 )  on  the  C-Surfacc,  the  minimum 
clearance  is  simply  the  vector 


sin(0  +  X,) 


) 


(*»  y)  (bxi  by)- 
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The  Minimum  Clearance  to  a  type  (b)  C-surface 


To  find  the  minimum  clearance  to  a  type  (b)  surface  and  the  orientation  at 
which  the  clearance  occurs,  we  let  g  be  a  type  (b)  constraint  (equation  (b J ))  and 
solve  the  system  of  partial  derivatives  of  H. 


=  2(i  -  bx)  -  t  cos(9iy) 

(1) 

IT 

—  =  2(y-6v)-^sm(<6y) 

(2) 

dH  ,,  . 

---  =  -/||«i||  sin(0  -  4>j  +  r/i) 

dH  ,  . 

—  =  -S(x,y,») 

(4) 

The  solution  is  considerably  easier  because  the  form  of  the  constraint  surface  is 
less  complicated.  Since  dH/dl  —  —g,  0  may  be  found  in  terms  of  x  and  y  using  an 
arccosine.  Substituting  this  value  of  0  into  (3)  yields  a  quadratic  equation  in  x,  y, 
and  l,  which  when  solved  with  (l)  and  (2)  for  the  following  roots: 

2  =  INI  C0S(N;  c°s(yy  -  7y) 

-  by  cos {4>j)  sin(<£y)  -  bx  cos(<£y)2  -  ||a,||  cos +  bx, 

V  =  INI  sm(0y)coa(<£y  -  qy) 

-  {bx  cos (<f>j)  ±  ||a,||)  sin(N  +  6„  cos(^y)2, 

£  =  2 1 1 by | ]  cos(<£y  -  7y)  -  2 bv  sin(^y)  -  26x  cos (4>j)  -  2||a,-||. 
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Appendix  II 

The  Connectivity  of  Configuration  Space 

H.l.  A  Review  of  Elementary  Homotopy  Theory 

In  this  appendix  we  review  some  elementary  homotopy  theory,  and  address 
the  connectivity  of  configuration  space.  See  Hocking  and  Young  (1961)  for  a  more 
extensive  review,  and  Donald  (1983a)  for  an  analysis  of  the  relation  between 
channels  and  homotopic  equivalence  classes.  Let  Ix  denote  the  unit  interval.  A 
parameterized  family  of  mappings  from  a  space  X  into  a  space  Y  is  a  continuous 
function  h  :  X  X  /'  — *■  Y.  Consider  the  mappings  /  and  g  from  X  to  Y:  v/e  say 
that  h  is  &  homotopy  between  /  and  g  if  for  each  point  i  in  X, 


h(x,  0)  =  f(x)  and  h(x,  l)  =  g(x). 

Intuitively  the  existence  of  h  implies  that  /  can  be  continuously  deformed  into  g 
without  leaving  Y . 

The  homotopy  relation  between  mappings  from  X  into  Y  is  an  equivalence 
relation  on  the  function  space  Yx .  llcnce  the  homotopy  relation  partitions  Yx 
into  disjoint  equivalence  classes,  which  are  called  homotopy  classes.  We  write  the 
homotopy  relation  as  /  —  g.  These  homotopy  classes  capture  our  intuitive  notion 
of  classes  of  paths.  The  homotopy  classes  of  YA  can  be  shown  to  be  precisely  the 
arcwisc-connectcd  components  of  Y*  (Hocking  and  Young  (1961)  ). 
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To  take  acoucrcte  example,  consider  configuration  space  for  the  two-dimensional 
mover’s  problem  to  be  the  product  space  of  the  2-dimcnsional  Euclidean  plane  9^ 
and  the  one-dimensional  sphere  51  to  obtain  9?^  X  51,  and  denote  the  configuration 
obstacles  as  CO  C  X  51.  Now  two  paths  f  and  g  in  the  same  equivalence  class 
must  belong  to  a  parameterized  family  of  mappings  such  that: 


k  :  X  X  /’  -►  3?2  X  S{  -  CO, 

and  /i(x,0)  —  /(a:),  h[x,  1)  =  g[x)  as  before. 

Now,  let  y  be  a  point  in  Y.  The  y  neighborhood  of  cyclic  paths  in  Y,  C(Y,y ) 
is  the  collection  of  all  continuous  mappings  /  :  i1  — ►  Y  such  that  /( 0)  —  /( 1)  ==  y, 
i.c.,  the  set  of  all  continuous  curves  that  begin  and  end  at  y.  If  /  and  y  are  curves 
in  C[Y,y),  we  say  that  /  is  homotopic  to  g  modulo  y  if  there  exists  a  homotopy 
h  :  i 1  X  I1  —*Y  continuously  deforming  /  into  g  without  leaving  V. 

Clearly,  homotopy  modulo  y  is  an  equivalence  relation,  and  decomposes  C(Y  ,y) 
into  disjoint  equivalence  classes  which  are  exactly  the  arcwiso-conncetcd  components 
of  C(Y,  y).  The  set  of  these  equivalence  classes  is  termed  the  first  homotopy  group, 
or  fundamental  group  of  Y .  We  say  a  path-connccted  space  Y  is  simply  connected 
if  the  fundamental  homotopy  group  for  X  is  the  trivial  group  of  one  element 
(for  some,  and  hence  for  all  y  in  Y).  Sec  also  appendix  III,  section  "Topological 

/■"i  i  :  *  -  }) 

Vuusii  imta. 

11.2.  The  Connectivity  of  Configuration  Space 

The  configuration  space  X  5'  (for  the  two-dimensional  mover’s  problem) 
is  not  simply-conncctcd,  since  Sl  is  not  simply-connectcd.  The  function  space 
(*"  X  5’)'  contains  several  homotopy  classes.  (3?2  X  5 1  )Y  may  be  envisioned  a3 
a  cylinder  on  which  there  are  clearly  two  classes  of  paths:  those  that  bound  a 
2-dimcnsional  region  and  are  contractablc  to  a  point,  and  those  that  go  around  the 
cylinder. 

The  configuration  space  5ft'*  X  50(3)  is  not  simply  connected,  because  50(3) 
is  not  simply  connected.  To  see  this  consider  the  following:  geometrically,  50(3) 
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is  homconiorpliu'  to  P'\  the  3-sphere  with  antipodal  points  identiDed.  As  is  well 
known  (see  Massey  (1967),  p.  166)  the  fundamental  group  for  Pn  is  cyclic  of  order 
2,  and  hence  Pn  is  not  simply  connected. 

General  configuration  spaces  (other  than  that  for  the  classical  Mover’s  problem) 
are  not  always  simply-connected.  For  example,  the  C-Space  for  a  manipulator  with 
six  revolute  joints  is  the  6-torus,  S1  X  S1  X  X  51  (to  6). 

Let  II  be  the  half-open  interval  [ — 7r,  7r) .  II  can  be  used  to  approximate  51, 
if  we  are  willing  to  tolerate  singularities  in  the  representation.  It  is  instructive  to 
generate  a  configuration  space  which  is  simply-connected.  Since  this  is  not  possible 
for  the  general  product  space  3in  X  S1  we  will  instead  consider  the  product  space  of 
92°  and  II.  Thus  for  the  two-dimensional  mover’s  problem  we  consider  the  product 
space 


C  =  5?2  x  n. 


For  a  manipulator  with  m  revolutc  joints,  the  C-Space  may  be  approximated 
by  IT™,  where 

rr  x n  x  ...  x n  (tom). 

The  throe  dimensional  rotation  group  60(3)  car  be  approximated  by  a 
hemisphere  of  the  3-sphere  (which  is  simply  connected),  or  by  IT’.  llm  is 
hoincomorphic  to  the  interior  of  the  m-cube.  This  new  product,  space  C  is  simply 
a  restricted  configuration  space  where  the  piano  is  not  allowed  to  spin  around 
wildly.  The  approximation  of  60(3)  by  a  hemisphere  of  S' *,  incidentally,  is  closely 
related  to  the  employment  of  unit  quaternions  to  represent  rotations.  The  space  of 
unit  quaternions  is  precisely  S'*;  the  two  quaternions  q  and  —9  construct  the  same 
rotation,  although  they  represent  antipodal  points  on  S'* .  When  all  antipodal  points 
q  and  —q  are  idcntiiicd,  the  projective  3-sphcrc,  P ’’  is  obtained.  P*  is  isomorphic 
to  60(3).  It  is  of  interest  that  liuier  angle  space,  Q*  (see  chapter  2)  is  essentially 
an  approximation  of  60(3)  by  IF’*  (mod  27r),  which  is  isomorphic  to  the  3-torus, 
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when  equivalent  rotations  are  identified.  If  equivalent  rotations  are  not  lentified, 
then  Q3  is  isomorphic  to  5J3.  While  the  approximation  of  SO( 3)  by  U  viclds  a 
simply-connected  configuration  space,  from  the  point  of  view  of  an  automated 
planner  it  has  the  undesirable  effect  of  introducing  an  infinite  number  of  goals  in 
the  rotational  dimensions  of  configuration  space,  for  every  single  goal  in  the  space 
of  Euclidean  motions.  For  this  reason  the  approximation  provided  by  the  3-torus 
may  be  considered  preferable. 


Appendix  1H 


Integrating  Local  and  Global  Algorithms  for  the  Find-Path  Problem 

In  Donald  (1983a),  wc  have  discussed  the  integration  of  a  global  channel 
algorithm  with  a  local  C-space  algorithm  to  form  a  planning  system  for  the 
find-path  problem  in  9?2  X  5l.  IIow  can  a  three  dimensional  global,  or  channel 
algorithm  be  coupled  with  the  planning  system  in  9J3  X  50(3)  described  in  previous 
chapters?  Mere  generally,  what  are  the  fundamental  issues  in  integrating  local  and 
global  geometric  planning  algorithms?  In  particular, 

(i)  How  can  a  global  algorithm  suggest  paths,  or  equivalence  classes  of  paths  to  a 
local  algorithm? 

(ii)  How  can  the  relevant  geometric  constraints  be  identified  and  exploited  by 
the  local  algorithm?  Conversely,  how  can  irrelevant  geometric  constraints  be 
effectively  ignored? 

(iii)  I  low  can  global  topological  constraints,  such  as  those  arising  from  analysis 
of  homotopy  classes  and  fundamental  groups,  be  propagated  onto  the  (local) 
geometric  structure  examined  by  the  local  algorithm? 

In  general,  the  design  of  a  global  algorithm  will  depend  on  the  geometric 
constraints  exploited  by  the  companion  local  algorithm  with  which  it  will  be 
coupled.  lienee  when  we  consider  extending  the  channel  algorithm  of  Donald 
(1983a)  to  the  three  dimensional  find-path  domain,  we  must  specify  what  ‘‘Large1./’ 
local  algorithm  to  use.  A  natural  candidate  is  vhc  }oca;.  algorithm  loi  find-path  in 
X  50(3),  which  v.t  desciibc  in  chapters  1  -2. 


Path  Suggestion.  A  problem  which  must  be  solved  in  any  local/global  Gnd- 
path  integration  is  how  a  global  path  may  be  suggested  to  a  local  algorithm.  In 
two  dimensions  this  was  accomplished  by  segmenting  the  find-path  problem  into 
a  sequence  of  sub-problems.  The  Suggcstor  strategy  (chapter  2)  is  designed  with 
this  in  mind.  The  verified  points  along  the  suggested  path  become  planning  islands 
in  configuration  space.  The  job  of  the  local  algorithm  is  then  to  connect  up  the 
planning  islands  and  find  a  continuous  path  to  the  goal. 


Choosing  Subgoals  in  Rotation  Space.  In  a  three-dimensional  rotation  space, 
the  problem  of  selecting  good  rotational  subgoals  becomes  more  difficult.  Ivva-li 
of  the  path-planning  literature  has  been  guilty  of  overlooking  this  difficulty,  i  uch. 
subgoals  can  be  used  in  path-suggestion  as  we  have  described  above.  Even  v/heri 
the  companion  local  algorithm  is  complete,  a  strategy  for  choosing  good  rotational 
subgoals  is  desirable,  since  it  would  allow  the  algorithm  to  converge  fruiter. 


We  have  derived  experimental  strategies  which  consider  alignments  of  the  robot 
polyhedra  with  the  faces  and  edges  of  obstacles.  Every  polyhedron’s  bouiulaty 
contains  ahgnable  generators  (tacc3  and  edges)  which  have  an  orientation,  and 
non-alignable  generators  (vertices)  which  have  no  orientation.  Two  geuei atois  are 
said  to  be  aligned  when  they  are  parallel,  and  the  rotations  in  which  thev  are 
aligned  form  connected  alignment  regions  in  SO(3).  For  example,  tvo  cubes  ar^ 
aligned  when  two  faces  are  parallel,  or  an  edge  and  a  face  arc  parallel.  Ihc  chan i:,t. 
construction  is  useful  for  identifying  the  obstacle  surfaces  which  bound  the  proposed 
channel  (in  real  space).  Call  this  set  of  faces  Let  7n  denote  the  faces  of  the 
robot.  The  alignment  regions  can  be  considered  for  the  generator  pairs 


Gjwgn  -  (.T«  X  *)u(j#  x  arK)u(a/«  X  ?<)• 

We  rro  only  interested  in  applicable  alignments,  that  is,  an  alignment  of  two 
(or  more)  generators  where  the  generators  can  be  brought  into  contact  through 
sonic  translational  motion  (this  is  our  definition  ol  applicability:  sec  chapter 
3).  Applicability  may  be  determined  by  examining  the  anplicability  constraint 
functions  (ACFs)  introduced  in  chapter  3.  Furthermore,  in  chapter  5,  we  showed 


that  alignments  of  edges  and  faces  occurred  exactly  at  the  boundary  of  the 
applicability  regions  for  C-functions. 

Every  C-func.tion  is  a  partial  function  /,  on  the  configuration  space  3?'1  X  50(3), 
whose  domain  is  X  Ai  for  Ai  C  50(3).  The  set  of  alignment  regions  is  obtained 
by  the  union  of  the  boundaries  of  these  applicability  regions 


for  every  C-funeikm  f;.  Every  point  in  Raiign  lies  in  the  kernel  of  some  ACF 
<7i(j  :  .50(3)  — >  3R  for  a  C-funcfcjon  /,,  In  chapter  4.  we  shewed  how  to  derive  charts 
for  these  boundaries  and  their  intersection  manifolds. 

In  most  hnd-path  problems,  the  alignment  regions  in  Ralign  are  complete  as 
subgoals  in  rotation  space — i.e.,  no  other  rotational  subgoals  need  be  considered  in 
order  to  find  a  solution  path  (ir  one  exists)  This  makes  a  certain  intuitive  sense: 
one.  might  try  aligning  a  large  Dox  with  a  nairow  door-  frame  in  order  to  squeeze 
it  through.1  However,  in  general  there  exist  pathological  cases  in  which  this  is  not 
true  (imagine  a  robot  which  looked  like  a  polyhedral  sea  urchin),  and  the  set  of 
alignments  is  not  complete  as  a  set  of  subgoals.  Furthermore,  it  is  unsatisfying  that 
the  alignment  analysis  exploits  strong  constraints:  in  the  polyhedra.1  domain  of  the 
classical  Movers1  problem,  and  Joes  not  appear  to  generalize  well  to  linkcd-arm 
problems. 

We  believe  that  it  may  be  possible  to  overcome  the  problem  of  ‘‘star-shaped”  robots 
by  considering  additional  alignment  regions  obtained  from  faces  and  edges  of  the 
convex  hull  of  such  objects.  Such  an  algorithm  would  have  to  deal  similarly  with 
“star-shaped”  obstacles.  Even  these  additional  alignments  may  prove  incomplete; 
however,  they  may  l  ave  heuristic  value. 

The  problem  of  how  a  global  algorithm  car.  infer  good  rotational  subgoals  from 
the  structure  of  real-i.paci.  is  one  of  the  most  interesting  open  problems  in  spatial 
planning.  We  conjecture  that  an  answer  may  lie  in  the  structure  of  the  boundaries 
1  We  arc  not  making  any  rl.miic  about  )nnn;:n  apnliu!  rcaMomnj'  her*-. 
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Figure  II  I.  Pathological  example  showing  a  robot  A  whose  alignment  regions  do  not  include  a 
rotation  which  helps  get  through  the  tight  passage. 

of  the  applicability  regions  in  50(3).  The  algorithms  we  provide  in  chapter  5  for 
obtaining  the  applicability  set  decomposition  may  prove  useful  in  computing  this 


structure.  Such  a  planner  might  slide  along  the  intersections  of  ACF  boundaries 
or  (eve/  ACFs  in  rotation  space, ?  much  as  our  planner  slides  on  the  intersections 


of  level  C-surfaces  in  configuration  space.  The  advantage  of  such  an  approach  lies 
in  reducing  the  (infinite)  search  for  rotational  subgoals  to  a  finite  combinatorial 
search  along  the  ACF  boundaries. 


Topological  Constraints.  Ideally,  the  global  strategy  should  enforce  the 
path-class  criterion3  for  each  sub-problem:  no  straight-line  approximation  for  a 
subproblem  may  cross  more  than  one  equivalence  class  of  paths.  We  begin  by  defining 
what  a  straight-line  approximation  means  in  9?3  X  50(3).  This  requires  some  way  of 
forming  “linear  combinations”  of  rotations.  The  definition  of  “linear  combinations” 

sLcvcl  ACIui  arc  defined  in  section  3.11. 

,See  and  appendix  II. 
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in  50(3)  relates  to  a  definition  of  convexity  for  3?'*  X  50(3).  In  particular,  wc  wish 
to  determine  whether  C-space  obstacles  are  convex.  For  if  C-space  obstacles  are 
convex,  then  wc  could  trivially  bound  the  number  of  intersections  any  straight-line 
trajectory  can  make  with  any  one  obstacle.  We  present  a  conjecture  that  the 
C-space  obstacles  are  non-convex.  Finally,  wc  discuss  basic  topological  notions  for 
formalizing  our  analysis  of  equivalence  classes  of  paths. 

We  require  some  w c.y  of  forming  “linear  combinations”  of  rotations.  The 
requisite  algebraic  structure  is  much  like  a  module  (over  the  reals),  except  that 
the  group  operation  cannot  be  commutative.'*.  The  group  operation  is  composition 
of  rotations.  Let  R(n,  0 )  denote  rotation  about  the  three  dimensional  vector  h  by 
angle  6.  Scalar  multiplication  by  a  £  3?  may  be  defined  by 


aR(n,  0 )  =  R(n,  aQ). 

ny  substituting  the  (non- commutative)  composition  of  rotations  for  the  group 
operation  +,  we  obtain  a  natural  definition  for  linear  combinations, 


aR{n,  0)  +  (1  -  a)R(h' ,  9')  =  R{h,  aO)R(n',  (1  -  a)0')  ( III.l ) 


for  0  <  ex  <  1.  R,  of  course,  may  be  conveniently  expressed  by  a  unit  quaternion. 

Suppose  Q3  *s  a  three-dimensional  parameter  space  for  50(3) — that  is,  the 
domain  of  a  chart  for  rotation  space.  For  example,  Q 3  might  be  the  space  of  Euler 
angles  (see  chapter  2).  It  is  possible  to  define  linear  combinations  in  the  parameter 
3pace  X  Q  .  This  seems  unsatisfactory,  since  it  makes  the  definition  of  linear 
combination — and  more  disturbingly,  of  convexity — dependent  upon  the  chosen 
parameterization  for  50(3).  Observe  that  definition  (III.l)  for  linear  combinations 
is  invariant  for  all  parameterizations. 

Open  Question:  Under  a  definition  of  convexity  invariant  for  all  parameteriza- 
tions,  show  whether  or  not  the  C-space  obstacles  in  3?*  X  50(3)  (and  SJ?2  X  51) 

*  Recall  that  a  module  in  defined  as  follows:  If  II  is  a  coin  imitative  rinj;  with  identity,  then  M 
is  a  module  over  H  if  is  a  commutative  group,  and  scalar  multiplication  (r,AY)  i-+  rM  of 

elcincn>  i  M  in  M  by  r  111  R  is  associative  and  distributive  (over  -I ),  and  if  \  uM  —  M 


arc  convex.  Conjecture:  We  conjecture  that  C-space  obstacles  are  non-convex. 
When  Si2  X  -S'1  is  approximated  by  SC2  X  [— 1 7r,7r)  and  embedded  in  5R3,  the  cor¬ 
responding  C-space  obstacles  are  non-convex  using  Euclidean  linear  combina¬ 
tions.  Furthermore,  in  both  SJ2  X  51  and  SR3  X  50(3),  each  obstacle  manifold 
is  the  intersection  of  a  finite  number  of  half-hypei  spaces  of  3?3  X  50(3). 
Each  half-hypcrspace  is  in  turn  defined  via  a  real-valued  partial  function 
/  :  5R3  X  50(3)  — >  3?.  Using  partial  functions,  arbitrary  non-convex  manifolds 
can  be  constructed.  Showing  that  the  obstacle  manifolds  could  be  represented 
by  means  of  smooth,  total  functions  would  suggest  convexity.  Our  analysis 
suggests  that  these  functions  must  be  partial,  which  in  turn  leads  to  the 
conjecture  that  the  obstacle  manifolds  are  non-convex.  | 

The  homotopy  relation  (see  appendix  II)  partitions  the  function  space  of  paths 
into  equivalence  classes.  The  image  of  one  such  equivalence  class  [/]  is  the  region 
in  C-space  covered  by  the  union  of  all  the  path  images  in  [/].  The  equivalence 
classes  are  determined  both  by  the  structure  of  the  underlying  C-space  ,  and 
by  the  C-space  obstacles.  Intuitively,  the  fundamental  group5  in  a  space  Y  is  a 
topological  invariant  corresponding  to  the  set  of  equivalence  classes  of  paths  in 
Y.  The  group  operation  corresponds  to  path  composition  (“pasting”),  and  for  two 
paths  /,  g  :  I1  —*Y  where  /(l)  =  g(0), 


/  *  9  (0  = 


U(2t-1), 


for  f  fn  1 1 

t,  2J 

for  t  6  [jj,  1]. 


We  think  of  /  *  g  as  the  path  whose  first  half  is  /  and  whose  second  half  is  g. 
The  pasting  operation  *  is  well  defined  on  path  homotopy  classes: 

[/]  *  Iff]  =  [/  *  9]» 


and  exhibits  groupoid  properties.  When  an  obstacle  makes  a  hole  in  frec-space,  it 
augments  the  fundamental  group  for  the  space  by  adding  an  infinite  (cyclic)  number 


8Soe  appendix  II  for  a  review  of  elementary  homotopy  theory  and  a  formal  definition  of  the 
fundamental  group. 
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of  equivalence  classes.  (For  example,  by  puncturing  the  plane  at  the  origin  O,  we 
obtain  the  classes  of  paths  (l)  not  looping  around  0,  (2)  looping  around  O  once, 
. . . ,  (n)  looping  n  —  1  times  around  0,  . . . ).  The  topology  of  the  underlying  C-space 
may  be  predetermined  (see  appendix  II),  but  each  new  find-path  environment 
generates  different  path  homotopy  classes.  We  wish  to  infer  the  equivalence  classes 
in  the  fundamental  group  by  their  generators,  i.e.,  the  C-space  obstacles.  Since  the 
C-space  obstacles  can  be  constructed  from  the  real-space  obstacles,  we  are  actually 
attempting  to  compute  path  classes  in  C-space  from  the  structure  of  real-space. 

In  general,  if  frcc-space  is  connected,  the  image  in  C-space  of  even  a  single  class 
of  paths  can  cover  all  of  free-space.  However,  we  can  impose  a  stronger  condition 
which  subsumes  the  path-class  criterion.  Let  p  be  an  injection  of  J1  into  C-space 


,  which  will  represent  some  approximation  of  a  solution  path  for  a  subproblem  S. 
We  wish  to  know  whether  the  image  of  p  can  be  expressed  as  the  union  of  two 


sets  of  points,  those  lying  in  the  image  of  one  equivalence  class  of  paths,  and  those 
lying  in  C-apace  obstacles  bounding  that  image.  In  chapter  4,  we  showed  how  to 
inter  set  trajectories  with  C-surfaces  in  5R2  X  51  and  SR3  X  50(3).  We  can  intersect 
p(/’)  with  all  C-surfaccs,  and  determine  the  intersection  points.  These  intersection 
points  indicate  where  it  penetrates  C-space  obstacles,  and  are  determined  from  its 
zero-crossings  from  frec-space  to  forbidden  space. 

Claim:  That  the  image  of  p  contains  either  no  region  or  one  connected  region 
lying  within  any  C-apace  obstacles  is  a  sufficient  (but  not  a  necessary)  condition 
satisfying  the  path-class  criterion  for  sub-problems. 

Constraint  Relevance.  Another  issue  concerns  how  a  global  algorithm  can 
characterize  the  relevant  constraints  for  a  local  algorithm,  and  if  necessary,  impose 
additional,  artihcial  constraints  on  the  problem  so  that  irrelevant  constraints  in 
the  initial  domain  will  be  ignored.  Because  of  the  difficulties  in  maximizing  channel 
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breadth  in  three  dimensional  channel  construction,  the  artificial  faces  introduced 
by  the  current  implementation  may  prove  too  restrictive,  especially  if  the  robot  is 
large  or  non-convex.  However,  the  channel  construction  is  useful  for  identifying  the 
obstacle  surfaces  which  bound  the  proposed  ci..  .>nel  (in  real  space).  Call  this  set  of 
faces  T&.  The  C-surfaces  generated  by  these  faces  may  be  exploited  by  the  sliding 
and  intersection  experts  (see  chapter  2).  Let  7/j  denote  the  faces  of  the  robot.  These 
preferred  C-surfaces  are  identified  with  their  generator  pairs,  namely 

Slide  =  | '  Tr  X  vert(7K)^\j(j}?lt  X  d7^\}{nert{Jti)  X 

The  identification  of  good  C-surfaces  to  slide  along  addresses  a  central  issue  in 
local/global  integration.  At  present,  the  sliding  and  intersection  experts  exploit  only 
local  geometric  structure  and  planning  history.  The  channel  algorithm  introduces 
a  global  criterion  for  selecting  which  C-surfaces  to  slide  along,  and  for  constructing 
intersection  manifolds.  The  set  of  C-surfaccs  Slide  specifies  an  implicit  volume  in 
configuration  space  which  is  closely  related  to  the  channel  volume  (in  C-space  ). 
This  volume  is  obtained  by  extending  the  hyperplanes  containing  the  C-surfaces 
past  the  obstacle  boundaries  until  they  intersect.  Furthermore,  Slide  lies  on  the 
boundary  of  the  image  of  the  hypothesized  equivalence  class  of  paths.  By  choosing 
these  interior  surfaces  as  candidates  for  the  sliding  and  intersection  experts,  global 
advice  on  constraint  relevance  is  provided  to  the  local  C-space  algorithm  by  the 
global  channel  algorithm. 
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Appendix  IV 


A  Listing  of  Macsyma  Code 

In  this  appendix,  we  provide  a  listing  of  the  Macsyma  code  to  produce  optimized 
Lisp  procedures  for  computing  th.e  coefficient  functions  of  tlic  cDP.onics!  lincnr 
forms  and  trigonometric  quadratic  forms  of  the  type  (a),  (b),  and  (c)  C-functions, 
and  type  (a)  and  (h)  ACF  clauses  for  3?3  X  50(3).  Using  these  forms,  the 
intersection  manifolds,  type  (c)  ACFs,  and  disambiguating  applicability  constraints 
arc  constructed  in  the  manner  described  in  the  text. 

We  also  list  (in  Macsyma)  the  resulting  combined  forms  for  the  C-functions  and 
ACFs.  Note  that  the  type  (c)  C-fnnclion  is  “over  a  page  long.” 

Rotations  are  specified  by  means  of  the  Macsyma  functions  RotatcVector{x  : 
vector )  and  Trans J orm( x  :  plane  or  vector).  Rotations  are  implemented  using 
the  Euler  angles.  However,  by  changing  these  two  functions,  any  representation 
for  rotations-such  as  quaternions,  spherical  angles,  or  wrist  angles  for  a  cartesian 
manipulator-may  be  employed.  This  corresponds  to  reparametcrizing  50(3),  and 
re  ults  in  different  charts  for  the  level  C-surfaces,  intersection  manifolds,  and  ACF 
manifolds. 


2-12 


/*  Display  and  Grind  function,  if  OP  f IN1Z£_FLAG  ts  TPUE  then 
■a  actually  stort  tha  OPTIMIZED  expression  •/ 

pr1nt("S«tt1ng  Grind  to  TRUE...*)* 

Grind:  True} 

Optinlze_nag:  TRUES 
/•  OPIMPRFF I X :  X  */ 

01splay„aftd_Grlnd(exp) :• 

Vlock([TabeT]. 

If  OPT 1MI ZE_f LAG  THEN 
(prlnt(EXP), 

Pr1nt("  Optlmiilng _ *). 

Exp:  OptlRlze(Exp) ) . 
label :Ld1sp(exp) , 

If  Grind  then 

(pr1nt(*  Ground,  becomes: 

*,  label[l).*  :  "). 

grlnd(exp)). 
pr1nt( '  *), 
lab#l[l])$ 


/•  Utility  function.  Is  the  expression  EXP  frte  of  all  tha  VARS  (a  UstT) 

Free_Of_V«rt(Vars,  Exp)  :■ 
block(ffre'cdom], 

freedom: true. 

(for  war  In  Vars  unless  freedim  ■  false  do 

(If  Not  FreeOf(var,  Exp)  than  freedom:  false)). 

If  Not  freedom  then 

Prlnt(’[Exp  contains  Major  Variables.  Recursively  Analyte...]"), 
frescos)!  . 

/*  here  we  define  Canonical  Linear  form  to  be  simply  the 
expression  of  the  constraint  as  a  linear  function 
In  X.  Y.  and  l  •/ 

Canon1cal„11near,variablas:  [X.  Y.  Z]S 

Canonical,! 1near_form(Exp)  :■ 

IsolateN(Exp,  Canonical,! 1near_varlables)S 


/•  Bruce  Donald  (BROBOZ)  analyze  hairy  expressions  MACSYHA 
•  little  bit  •/ 

/•  Analyze  3U intar  forms  :  given  tht  ’chief  tars’  in  RATVARS. 
generate  intermediate  labels  for  ail  the  coefficients 
of  these  tars  and  return  the  "simplified’  bilinear  fora. 
Recursively  Calls  ANALfU _BlUNEAR_fCmK  So  that  the  intermediate 
labels  are  truly  ’constants’  relative  tc.  the  Ratvars  •/ 

/•  typically.  r»t*ars;[x.y .z.psi .theta. phi]  '7 

/•  IsolateN  worts  like  JSOtaif  but  for  N  variables  in  a  list, 
on  a  bilinear  fora  •/ 

1*clateK(Exp.  Nvars)  :■ 
tiock([Save_Ratvars ,  Ifora], 

Savc.ratVars :  ratvars, 

RatVars:  Mvars. 

x  Ifnra:  Antlyz-#_Bi1 inear_for*(C*p) . 

Rfctvsr  s :  Savajiatwart. 
lfora)5 

simple_Araly»e_C»plh:  4$ 

Analyie_B11inear_rora(e*p',  :» 

block([po«ar.  Coef.  Rat_fxp.  Left,  loss,  Sun.  Label]. 
print( "Analyzing:’). 

1  dispj  reveal  i  Exp.  si(Rple_anslyze_dapth}). 

Left:  rat(exp),  Sum:  0. 
for  tar  In  Ratvars  do 
(Power:  Hipow(Left,  Var). 
if  power  >  2  then 
(lose:  var'power. 

trror( "Wermng :  Not  a  Bilinear  Form  because  of  * 
Coaf:  ratcoeff(teft.  tar.  2). 
if  not  (Coef  ■  0)  then 

(  /r1nt(  *ihe  r.oe'. ficlent  of  r,  tar*2,"  Is  “). 
label :Di»pley_and_gr1nd(Coaf ). 

If  not  fr«ce_of_tars(ratvare,Coaf) 

then  Label:  Ana1yzeJ8nin#ar_*or»(Coef). 

Sun:  Sum  e  label  •  Var’2, 

Left:  rat(left  •  Coef  •  (Var  *Z)))). 
print(’»1*ed  teraa: 
for  tar  in  Ratters  do 
(for  tar?  in  Rattan  da 
if  tar  fvarZ  then 

(Coaf;  retcoeff(L*ft,  tar*ter2,  1), 
if  not  (Coaf  •  0)  then 

(  Print(  "Tht  coefficient  of  ",  Var*tnr2,"  1*  *). 
lace!  :Diip1ey_ariS_grir.S(Ccef ) . 
if  Not  Fre6_0f_v»rs(RatVars,  Coef) 

then  label:  Aftelyz*_8iHneai_forw(Coaf ) . 

Sum:  Sum  +  label  •  tar*  var2, 
la*t:  rat(loft  -  Cozf  ’  (Var  *  var2))))). 
pr1nt(*  Linear  Terms:  *), 
for  tar  In  Ratvars  do 

(coef:  ratcotff(left,  tar,  1). 
if  not  (coaf  •  0)  then 
(  print(’Tha  Coefflciart  of  ",tar."  1$ 
lebtl :Display_and_gr<na(Cotf) , 
if  Not  F ree_Of_vars(RatVars ,  Coaf) 

then  Label:  Analyze_Bil  inear„.for»(Co*f ) . 

Sum:  Sum  *  Tsbel’ver, 
left:  rat(left  -  Coef*  Var))), 
if  U.  t  #  0  then 

(Print("  And  the  constant  term  is  ") 
laoel :  Oisplay_and_grind(left) , 
jura:  Sum  *  label) , 

Print("  Yielding  :’),  D1spley..end_5rtnd(SuM) ,  Sum)S 


* ' 


/•  Bruce  R.  Donald.  (BRDB02)  Mode: Mac lyma 

Attempt  to  exprtts  applicability  constraints  for 
C-Surfocas  in  «*3  \cross  $*3  */ 

/*  PRODUCT  I  ON  VERSION  *-  1.*..  for  production  of  UJ>P  coda  •/ 

BTHETA:  [phi,  theta,  psi]> 

If  .  Culer_Rot.*tion_equations_1o»ded  ■  TRUE  then  ’OK" 
else  Batchload  ([rotate. nac]) ; 

thorten(enp) :•  subst(  S.tin,  subst(C,  ros,  exp)JS 

ratvars : [  a in(phi ) , cos (phi ) ,sin( theta). cos( the va) .  sin(psi) , cot (pal)]; 
/*  Etch  constraint  it  of  the  Torn  -i 


/•  vectors;  •/ 

u(i)  :•  [ua[i].uy[i].uz[<]]; 

v(i)  :■  [vx[1 j,vy[i],v2[i]3; 

/•  Norn.l  fur  plane  eq  •/ 

«(i)  :■  [r,x[i],nyti].nji[i].nd[i]]; 

/*  Were  «e  define  functions  to  genaratt  the  applicability  constraints, 
the  arguments  art-  Bn  :  a  vertex  in  3-zpace,  which  we  use  to 

measure  distance  to  the  plane. 

v  :  e  vertex  «fucn  «•  insist  must  be  ON  the  plena. 

H  ;  a  plane  (4-veetor) 

c  :  the  ’height*  of  the  level  surface  in  S’3. 
if  0.  corresponds  to  the  mar.imim  boundary 
of  th«  applicability  clausa  (ag  edge-face 
contact). 


R3_projection(V#c)  ;•  [v#c[i],  vac[2J.  vec[333$ 

Typa_B„Clause(t!.  bn,  v,  c)  :■  ' 

«1rep_3( 

(R3_Project1on(N)  .  Rotate_vector(Bn)) 

*  (R3_Project1on(*)  •  Rotate_v#etor(v)) 

-  c); 

TyPft_A_t'l»use(N.  br.,  *,  c)  :• 

Bl0Ck([«„THETA3, 

H_TH£Tft:  p».rt(Lransform(Ecl*r_lnversi!,  B).  1). 
timp_3(, 

(R3_.Prnjattion(N.„THETA)  .  bn) 

*•  (S.3„Prri jectii}n{N„.th«t*,,j  .  v) 

-c)); 

Midp(ii,fc)  si  *  (b-a)f.U 

typo„Cl.„Cl»uso(Bl,  i)2.  al,  »>’,  tl,  c.2)  :• 
sin'»..;l( 

type_b,.tl*'use(nl ,  al,  midptal.aZ),  Cl) 

“  t^pvfl.,b,..tiiiuse(n2,  al,  »1dp(al,aZ),  c2 ) ) ; 

«SJi.*»(fc3 ,W!f .  bi,  b2,  el.  c2)  :■ 

sii»p..3( 

*  typt_a„c1aiisa(nl ,  bl,  «idp(bl,b2).  cl) 
v  typ«„t^.cla;;ie(n?  .  b2,  midp(bl ,b2) ,  e2)); 

fn  Itvfi^  1*  just  a  CONSTANT  to  construct  a  Laval  surface  on  S’3  which 
is  appliceble  */ 


v.  mm 

(W*  \ST*  .V*- 

mm 


Xt.  VC,  2C.  VC  «r*  £'J0.  fufictiefti  (KACftCS)  ♦*»  *2 


*yp*  YC(5!}. 

[*<(«) ,  jrc<u), 

{v{u)T  sci»>2. 

j ; 


bfJ’ 

61*1 


typ#_i)_c1ti«;s«(  *{«).  ^ t  J . 

typ#„ti_c'6use(  *{J2Jt  n{U)i  t**£Z]i)  / 


Type_*_*cl9Ui»n3: 

typ«-*_t1cus*(!>c{«).  YC{«).  2C<a).  *l(»05- 
t*c(nV .  yc(u>.  JC(U)1. 

[»?.{«/.  yc/>).  *c{v)}. 

lovil); 


/E 


»[1]  :  «{U). 

«[23  :  lyPO_»_c1*u*§(  b(12). 


u(»l),  »(pl). 

o(«2).  *{p2).  *f 


/•  C_1  [  V  21 
C.2[l] 


type_d_c'!«us*(  n(fr3). 
type_c2^c1»u**i  •*(•»). 


l*vI63) 

u(bj:).  u(bJ2).  2ty[V]. 


/•  grind  the  result*.. .  •/ 


Typ«_^.Ac1#u**[l3  :*  )S  6r1«d(Typ*_A_Ac 
Tlp»i*cl8UMtl]  s  *)$  Srmd(l*P«JL>cl«u*»IIJ)S 


/*  ,br*.c#  r .  jonsld.  cspate  constraints  In  3-fiiiti*nsions.  -lies ym 

7'  fi*oi!uct1on-»if*'on:  i.a.  produce  IHp  coda... 

■'  yc.  and  *c  ure  baccessor  wmctos  for  components  of  tractors  V 

/»  this  #c*t  section  contains  the  equations  for  r.-».u-fer*i5  in 
i  *3  *  s'1*,  sea  aim  SOS,  tcnas*  apctinl  planning  paper  fwr  details.  •/ 

/•  to.id  aular  rotation  equations  •/ 

If  auler„»'f  l.ticn.equatiosic.loadad  «  tru* 

■'  "ok*’ 

jJrs  {iiatcMoiai'oirdSifu.-U.prodjrotate'.utac''}. 

•tiler. .rotation.*!}!  r tions.loainfi:  lru«) ; 

l  • 

/•  a  vector  is  r*3:  •/ 
iivtc  :  i>,  «f„  *]): 
rct;srs-  [*.  y.  a. 

»ir.(pM),  cosfpri),  s 'n(  tfirtii),  cf>t(»h*i.») . 
sin(psi).  toirpsl)  3 ; 

/»  «  on  a  ,  cud  •/ 

*1  •  l[«c{*1)  .  yc(ii'<),  ic{at)3: 

aitrns:  jetton*},  icfaior.a)].- 

/*  «  vert#*  on  i  .  and  &{J+1)  */ 

ty.  rr{t/j)3; 

Dions:  [tctojnna).  yc(i)jnM),  aetbjuna)}; 

/•  cm i!  norniti  tc  &  »*c«  ii  oti  r  */ 

nti;  [  »c{r.fi).  yc(nfl).  ac(r.fi),  we{nfi)3; 

/*  fh*  tngt-wal  *o  t  fata  54  on  b  *V 

ngil  5  [xc(«t:l].  ?'.-.(nej).  fc(-ipj).  «£(«9j)3; 

/*  typt  a  si'r'nct.  rotst.j  tiw  no'-*s^;  •/ 

W.prnitctlonjVecj  :•»  [v*».£l3,  *•£[*].  sqrc£333# 

B'  t_Nf1:  p»rt<ir«nsfort»{Cui*r  .If  «er  sa.Ri  5).t); 

M  m  •  B1  nm4ftetfA>./RB«  if#  1  >  • 

•  •'*»— *  1  ''J'  *  »  i  •'»  »•"  1  rf  f 

Inner.»'r«duct_T»r**:  (ftota*e„v*ctar(si)  >  bj); 

A.6:  «_a  .  r./ac  *  (K_«t  .  Inn®r„Pvodgct^T«r»i) ; 

/•  Type  S  Surfacs  •/ 

St.**:  'i3.pt  oJact;oo(NgJ ); 

fc_6  :  h_b  .  Xvat  -  (M_b  ,  Innar_Pro£ii»ct.Ttr») ; 

/•  Type  C  surface  (S)  •/ 

Tdja.a:  rat»tt_vactor{aiona)  -  rotsta.,vactor(ai): 

Edg«„b:  BJon*  -  bj; 

K.C:  Cross(  Edg»_e,  Edp18-^); 

*_C  .  Xvtc  -  (B_c  .  Inner_Product_Tarw) ; 


s-sb  } 


A_S:  Si«p_3(r»'-*<«P(^'6^ 
C  5;  Slnp_3(''»t*i**PlB-6)) 

C;6;  Sinp_3(r»U1wp(C„5)) 


penStH(aJs).  l*notblC^6)]: 

/«  Grind  tit*  fern*  h*r«  •/ 


l>P*lLt*»»^««Il3  *  Brl«a(0)S 

type^B-Csurfatenl  :')*  0rind{B_8)S 

Tjp«lc_C*urfsc«[l]  :')*  G» ind(C_S)S 


-  «  . 


/•  MODE :  MACSYHJ. 

p.kc  m  pts  tun  ions  01  type  <a\  («)  and  <c)  c-surfaces  tor 

TNI  SOOf  NO  Vi  FsS  PROBLEM.  AND  TYPE  (A)  AND  (8)  APPLICABILITY 
CONSTRAINTS  .  Ovtpiit  Of  CSPA Ci  and  APPLSC  under  PRODUCE  for 
product  Ion  run.)  •/ 


Tjp*_ftJE*ur»ect[l]  : 

{((XC(Nri)*K-KC(ej)»XC(Nf  I))*C0$(PH5)<(XC(Nf  I)*Y-YC(BJ)*XC{Nf  l))*SINiPHI)) 
•COS(TMETA) 

-♦(-1SC(N<  I)*2-»2C(  CJ)I,XC(NI  I )  )®a3N(  THF.IA)r(YC(Nf  I)*V-YC(BJ)“YC(Nf  1  ))*COS(PHl) 
♦  (-YC(tin)»X*»C(CJ)*YC{Hn))®SIN(Pi!l;-YC{llfI)) 

•COS(PSI) 

+i{(-YC(«FI)»X-XC(BJ)*YCf«f l))»COS(PHI) 

•♦( '  VC{Rf  T  )*Y*YC(BJ)*VC{N*  I  ))'SIH(PHI)) 

•COS(TKETA) 

*(FC(NFI  )*2'2C(8J)*YC(NfI))*SIN(THEtA)-»(XC(Nfl)*Y-YC{BJ)*XC{NFI)),,COS(PHI) 
•*(-XC(Nn)*X-XC(BJ)«XC{Nf  I))»SIN(PH1)-XC(HFI)) 
•SIN(PSI)®(ZC(Nn)*Z-ZC(6J)*2C(Nf  1)  )*COS{ THETA) 
*((ZC(NFI)*X-XC(8J),2C(Nn))*COS(PHlJ'*'(2C(Nfl)®Y-YC(8J)*2C(NFl))*5IN(PHI)) 
*SIN{THUA)-2C(AI)®2C{NF1)'YC{A1)«YC(NFI)-XC(A1)*XC(«IFI)$ 


TYP«_B„C*urfac#[l]  : 


((-XC( AI )*XC( NGJ)*COS(PHJ )-XC(AI)*VC{NGJ) *S1N{PHI ))*C05(TkETA) 

■♦XC(  Al  )*ZC(N6J)  ®SlN(  THETA) -YC(AI  )*YC{NGJ)*COS(PHI )*YC( AI  )*XC(NGJ)*SXN(PHI)) 
•CGS(PSI) 

♦{(YC(AI)»XC(NGJ)*COS(PHl)-YC(AI)*YC(NGJ)»SIM(PHI))«COS(THETA) 

*YC( AI )*ZC(N60)*SIN( THETA) -XC(AI )*YC(NGJ)*C0S( PHI )®XC(A1)*XC(NGJ)*SIN(PHI)) 
•S!N{PS!)-ZC'A!)»ZC{NG.n»eCSf  THETA) 
•rl-ZCiM)*XC(NGJ)*COS(PHi)-2C(AI)*YC(N&J)*£lN(PHI))*SII!(THETA) 
-YC(NGJ)*COS(PKI)-XC(N&J)*SIN(PHI)*IC(NGJ)®2-YC(NGJ)»Y*XC(liGJ)*X 
-ZC(BJ)*2C(NGJ)-YC(BJ)*YC{NGJ)-XC(BJ)*XC(N6J)$ 

TjfPL^r,„c*urf«cnr.l]  J 


( ( ( ',  (KC( A10NE)”XC ( AI )  ),YC(BJONE)*(“XC(  AIONE  )*XC(  A1))*YC{BJ)  )*Z 
*{(-KC(ATGHE)*XC(AI))»ZC(0JONE)»(XC(AlONt)-XC(AI))‘ZC(BJ))*V 
»(XC(AIONE)'XC<AI))»YC(BJ)»ZC(BJONE)*(-XC{AIOKE)rXC(Ai  ))'ZC(BJ)*YC(BJOIiE) 

■r  ( -YC(AI  )®2C(  A'.Ot'E  )*ZC(  AI)*YC(  AIONE  )),XC(BJ0NE) 
'r(YC{AI)«ZC(!UONt:)*ZC(AI)®YC(AIONE))*XC(BJ)) 

•COS(PHI) 

*(  XC:  MOMt  )<-«(  AT  ))»XC(BJ0»E)  +  (XC{  ATONE  )-XC(AT))»XC(BJ))*Z 
♦((XClAIONLj-XCfAIJj'ZCJBJONEl^l-XUAiOiltjriCiAIji^CvBJ))** 
<('XC(AIONE)+XC(AX))»XC(BJ)*ZC(BJONE) 

4  (*YC(*I  )*ZC(AI0:U'  )+ZC(  AI  )*YC(  AlONE ) )  •YC(BJ0BE ) 
4(XC(AIO*<E)-XC(AI))«ZC(BJ)«XC(BJONE) 

VC( AI )  “ZC(  AIORIE  )~ZC(  AI  )®YC(  AlONC  })*YC(BJ)) 

•SIN(PHI)*(XC(AIONI )-XC(A:))»ZC(BJONE)+(-XC(AIO«E)+XC(AI))»ZCt*J)) 

*CQ5( YKETA) 

+  (((XC(A10N1  )-xr.(A’i),ji*\C(BJONE)'r(‘XC{AIO«E)+XC(Ai))»XC<BJ))»COS(PNI) 
+((XC(AIONE)-Rf.(Al)>  YC(BJONE)®(-xr.(niONE)*Xr.lAi  •  )»YC(BJ))*SIN(P«Z) 

*((  'XCfAIOJIE  )»XC(A)  ))*XC(PJONE  )*(XL( AiONE  )-XC{  AI  ))*XC{BJ))*Y 
♦((XC(AIC  E  )*!!('. {Al )  )*YC{BJ0NE  )♦(  'XC( AJONE  )+XC(AI)),YC(BJ))*R 
♦  (  YC(  AI  )*ZC(AIOfIE)-2C(AI  )*YC(  AIONE )  )®ZC  (B  JOHE) 

-»(-XC(A10NE)*XU  AX))»XC{BJ)*VC(eJOHE)nXC(AIONE}-XC{AI))»Y  U)*XC(BJ0NE) 
•♦(~YC{AI)»IC(AXG«F.)  <2C{AJ)‘'YC(  AIONE))*2C(80)) 

*SIN(THITA) 

*(((  “YC{  AIOBI  )+YC{  AI  ))’'XC(BJOKE  )®(YC(A10NE  )-YC( AI ) ) "XC(80})*2 
+  ( (YC(Al  ONI  )-YC(Al )  )*ZC(BJONF)r/-YCiUONE)  +  YC(Al))*EC(BJ))eX 
*{-fC{A10KE ^YC(AI)  V'XC(BJ)»ZC{BJONE) 

+  (XC( A) ) *2C( ATONE ) -I C{Ai)*XC( AIONE ))*YC( BOONE) 

■*  ( YC(  AZOV*.  )-VC(  AS)  )"2C{BJ)*XCf  BJOKE ) 

+(•  XC ( M  )*/.C( AlOHE )  »2C(M  )*X(.(  AIONE ) )*YC(BJ) ) 


•COS(PHI) 

f( ((-*C( AIONE )+VC(AI))«YC(BOONE)«<YC(AIONC)-YC(AI))*YC(BO))*2 
*((VC(AION£)-VC(AI))*ZC<BOONf  )*(-VC(A10NE  )*YC( AI ) )*ZC(B0) )*V 
•{* VC (AIONE  )*YC( AI )  )*YC(BO)*ZC(BJONt.)  +  (YC(  AIONE  )-VC(AI )  )*2C(60)*YC( BOONE) 
+(-XC{ AI  )'ZC( AIONE  )«7C(AI)  “AC  (AIONE )  )*XC( BOONE  ) 

•(XC{ AI  ) *2C( AIONE }*2C(A1  )*XC( AIONE  )  )*XC(G0) ) 

*51N( PHI ) ) 

•COS(PSI) 

•*({(<  (-VC(AIONE)*VC(AI))*YC(BOONEWYC<AIONE)-YC(AI))*YC(BO))*2 
♦({ VC{ AIONE )-YC(Al))»2C(6J0NE)*{-YC( AIONE )+YC( A1 ) )»ZC(BJ) )»Y 
«(  -YC( AIONE  ) ■» YC { A 1 )  )*YC(BO)*2C(BJON£  )+(YC(  AIONE  }-YC(  AI )  )*ZC(EJ)*YC(BOONE) 
♦(-XC( AI)*ZC(A10NE)*2C(AI)*XC( AIONE ))*XC(BOONE) 
♦(XC(AI)-IC(AION£)-ZC(AJ)*XC{AIONE))8XC(BO)) 

•COS(PHI) 

+((( YC( AIONE )-VC( AI ))8XC(8JON£)*(*YC{ AIONE )*YC(AI))8XC(B0))*2 
•((-YC(AIONE)*YC{ AI  ))8ZC( BOONE  )+( YC( AIONE )-YC(AI )  )8ZC(BJ))*X 
♦( YC( AIONE )-VC(AI )  )*XC(BJ)*ZC (BOONE) 

-KC { A 1  )*ZC( AIONE  )+2C(  AS  )*XC(  AIONE )  )* VC (BOONE) 

•*(*VC(  AIONE  )*YC( AI )  )87C(BJ)8XC(BJONE ) 

+(XC{AI)*2C( AIONE )-ZC(Al)8XC( AIONE ))»YC{8J)) 

•SIN (PHI )♦(- YC( AIONE )*YC(AI) ) 8ZC(8 JONE )+( YC( AIONE )*YC(AI))8ZC(BO)) 

•COS( THETA) 

♦  ( ( ("YC( AIONE )+YC(AI ) )8XC(BJON£)+( YC( AIONE )~YC(AI ) )8XC(BO))8COS{PHI) 
•((*YC( AIONE )+YC(AI))‘YC(BJONEj+(YC( AIONE )-YC(AI))*YC(BJ))»SlN(PHI) 
*((YC(AIONE)-YC(AI))*XC(BJON£ )*(-YC( AIONE )+YC(AI))8XC( BO) )*Y 
+((-YC(  AIONE  )+YC(Al))*YC(BOON£)<-(YC(  AIONE  )-YC(AI))»YC(BO))»X 
+(XC(AI )*2C( AIONE )-2C(AI) *XC( AIONE ) )*2C( BOONE) 

+( YC (  AIONE  )~YC(AI )  ) 8XC( BJ ) 8 YC( BOONE )*(-YC( AIONE )*YC(AI))*YC(BO)8XC(BJONE) 
+(-XC(AI ) *ZC( AIONE )+ZC(Al )*XC( AIONE ) }*2C(BJ}) 

•SIN(THETA) 

•((('XC(AIONE)*XC(AI))»XC(BJONE)*(XC(AIONE)-XC(AI))»XC(BJ))*2 
+(( XC( AIONE )-XC(AI )) 82C (BJONE)+(*XC( AIONE )+XC(AI))*2C{ BO })*X 
•E -XC( AIONE I+XCf  AI 1  l*XCf  BJ1*2CE BOONE I 
+(-YC(AI)»2C(AIQSEj-2C(Al)»YC(AI0NE)jeYC(iJ0SE) 

+( XC ( AIONE )*XC(AI ) ) *ZC ( BO )*XC( BOONE) 

*(YC( AI )*2C( AIONE )-2C(AI) *YC( AIONE ))*YC(BJ)) 

•COS(PHI) 

♦(((-XC(AIONE)*XC(AI))0YC(flJONE)*(XC(AIONE)-XC(AI))*YC(BO))»Z 
+( (XC( AIONE )-XC(AI ) )8ZC(BJONE)+(-XC{ AIONE )*XC(AI) )*ZC( BO) )*V 
♦{-XC(AIONE )*XC(AI ) )*VC(8J)*2C(BJONE )+(XC( AIONE )-XC(AI ) )*2C(BJ)*YC(BJ0NE) 
+(YC(AI )*2C( AIONE )-ZC(AI )*YC( AIONE) )*XC( BOONE) 
t(-YC(AI)-2C(AIONE)*2C(AI)'YC(AIONE))»XC(BJ)) 

•SIN(PHI)) 

•SIN(PSI) 

♦((( -ZC( AIONE )+ZC( AT ) )*XC ( BOONE ) +(ZC( AIONE )-ZC(AI))*XC(BJ))8CO$( PHI) 

+( (-ZC( AIONE )*2C( AI ) )*YC(BJONE )+(ZC( AIONE )-ZC(AJ) )*YC(BO) )*SIN{ PHI) 

■♦•(  (ZC(  AIONE  )-2C(AI ) )*XC( BOONE )+(-ZC{ AIONE  )*ZC( AI ) )8XC(BJ))*Y 
♦((-*C( AIONE )+ZC(AI))»YC(BOQNE)*(ZC( AIONE )-ZC( AI ) )«YC(BJ) )•* 

♦(-XC(AI ) *YC( AIONE )+YC(AI)*XC( AIONE) )82C( BOONE) 

♦(2C( AIONE )-ZC(AI ) )*XC( 6 J)*YC( BOONE )+(~ZC( AIONE )+ZC(AI) )*YC(BO)*XC(BOONE) 
*(XC{ AI )*VC( AIONE )-YC(Al)»XC( AIONE) )*ZC(B0)) 

•COS( THETA) 

♦( (((?C(AIOHE)-ZC(AI ))*YC(BJONE)+(-2C(AIONE)+2C(AI))cYC(IO))*2 
•( (-ZC(AIONE)-ZC(AI))-ZC(BOONE)+(ZC( AIONE )-2C(AI))»ZC(B0))*Y 
+( 2C ( AIONE )-2C(Al ))8YC(BJ)8ZC (BOONE )+( -ZC( AIONE) ♦ZC(AI))*ZC(BJ)*YC (BOONE) 
♦(-XC( AI )»YC( AIONE )♦ VC (AI )*XC( AIONE ) )*XC( BOONE) 

•  (XC( AI )»YC( AIONE )”YC(AI )*XC( AIONE ) )'XC(BJ) ) 

•COS (PH I) 

♦( (( *ZC( AIONE )+ZC(AI ))*XC( BOONE )*(ZC{ AIONE)*ZC(AI ))*XC(B0))*2 
♦((ZC(AIONE)-ZC(AI ))*ZC( BOONE )+(-ZC(AIONE)*2C(AI)),ZC(BO))*X 
♦(-ZC( AIONE )+ZC(AI ) ) *XC ( BO )*2C( BOONE ) 

*(-XC(Al )*YC( AIONE )«YC(A1 )*XC( AIONE) )*YC( BOONE) 

♦{ 2C( AIONE )-2C(AI))»ZC(BO)*XC(BOONE) 

♦( XC( AI )»YC( AIONE )-YC( AI)8XC( AIONE ))*YC(B0)) 

®SIN(PHI )♦( ZC(AIONE)-ZC(Al ) )8ZC( BOONE )♦{ -ZC( AIONE )+ZC(AI ))*2C(BO)) 
•$IN(TH£TA)S 


/•  Harar  art  tha  Applicability  conatrainta  •/ 


Typ«_A_Ac  1  «us#[  1]  : 


<({*C{U)-XC(V))*XC{»)*COS(PHI)MVC(U)-VC(V))nC(K)*SU!(PHI))»COS(THETA) 

+  (-2C(U)*2C(V))*XC(*l)*Slll(THETA)«{YC(U)-YC(V))*YC(*)*COS(PKl) 
♦(-XC(U)+XC{V))»YC(«)*$I*(PHI)) 

•COS(PSI) 

♦{((-XC(U)*XC(V))'YC(H),C0S{PHI)'H-VC(U)*YC(V)),YC(M)*S1H(PHI))‘C0S(THETA) 

♦  (2C(U)-2C(V))*YC(N)*SIIi(lHEIA)*{YCiU)-YC(V))»XC{«}*C0S(PHl) 
♦{-XC{U)*XC(V))‘XC{N)*SYH{PHI)) 

*SIW(PS!  )'•'(  2C(U)-2C{V)  )*2C(N)*COS( THETA) 

♦  ({XC(U)-XC(V)),2C(N)»COS{PH|)«(VC(U)-YC{V))«2C(N)»SI»l(PHl))*SIN(IHETA)-L£WELJ 


Type_®_Acl«g*t£l]  : 


(((-XC(N)*XC(V)*XC(M)*XC{U))»C0S(PMI)*(-YC(K)»XC(V)*YC(I«)»XC(0))»S1H(PM1)) 

•COS(THETA) 

+  (ZC(N5«XC(V)-2C(N)1'XC(U))*SI»l(TH£TA)4(-YC{ll)*VC(V)^YC(ll)oYC(U))*C0S(PHn 
+(XC(K)*YC(V)-XC(K)*YC(U) )*SIN(PHI) ) 

•COS(PSI) 
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♦ 


(((XC(N)'YC(V)-XC{N)*VC(U))«COS(PHI)  +  (YC{ll)*YC{V)-YC{l»)*YC(U))»SINfPHn) 
•COS( THETA) 

+  (-2C(N)*Yr(V)*2C(A)»YC(U))*SIN(mTA)<-(-YC{«)*XC{V)+YC(H)*XC(U))*COS{PHI) 
+  ( KC(N)*XC(V)-XC(N  )*XC(U)  )*SIII(PHI ) ) 


•SI«(PSI)-(-2C(H)*2C(V)*2C{N)*2C(U))»COS(THETA) 

( (*XC(N)*2C(V)*XC(N)*2C(U) )*COS{PHI )+(-Yt(N)*2C( V)+YC(M)*2C(U) )*SIN(PHI)) 
•SIH(THFTA)-LEVELJ 


/•  MO0«:  MUIJfSI  *•* 

(Bruce  Donald.  Hare  «•  Express  a  Constraint  (C-surftcc  or 
Acf)  In  Canonical  Linear  torn  and  Canonical  Trig  fora. 

Gluon  a  CONSTRAINI  which  Is  either  a 

C-surface  or  an  ACf  (applicability  clause  function) 

and  a  variable  (VAR)  wo  solve  for  the  variable)  •/ 

Load_up():»  /•  load  necessary  files  */ 

b1ocfc([j.  /•  for  solving  •/ 

If  All _ DCf S_L0ADE0  ■  TRUF  then  "OK" 

else  batchload(~usrd}:[brd.prod3def abc.aac") . 
batch 1 oad([ intabc ,nac]) . 
bstchload(nusrc!S :  [brd. prod] anal yze2  .mac") , 

Ratvars:  [i,  y.  i. 

sin(phl).  cos(phi),  cin(  theta),  cos(theta), 
sin(psi) .  cos(psl)  ]. 

Angles:  [Phi.  Psi.  theta])$ 

Load_ui()S 

/*  give  us  an  ’explicit"  tangent  space  »/ 

s(var)  :«  t1n(var)S 
c(var)  :*  cos(var)S 

Bui1d.Man1fe1d()$  /•  Rebuild  Nanifold  */ 

solve_for_engle(e*p, var):»  /•  Solve  for  COS(var)  •/ 

bloct([Rats,  Rl,  R2,  Rlj. 
rats:  ratvars, 

Ratvars:  [c(var).  t(var)), 
prints "Simplifying. . ."), 

Rl:  IsoiateN(exp.  Ratvars). 

Frfnt(*tl  ininatlng  ",  s(var).". ..'), 

R2:  E1i»inatt[l](Rl,  Var). 

Ldlsp(R2). 

Pr1nt("$o1v1ng  for  *.  c(var).", 

R3:  solve(R2,c(var)). 
dispiey_end.grlnd(R3), 
reivers :rats, 
r3)$ 

Sol vi_for_X(E»p.  Avar):*  /•  Solve  for  Any  Var  •/ 

Block([Rets,  rl,  R2], 

Rats:  ratvars. 

Ratvars;  [Avar], 
pr1nt('Simpl Ifylng. . ."). 

Rl:  lsoiateN(txp,  Ratvars). 
pr1nt("  Solving  for  ",  Xvar,*..."). 
r2:  solve(Rl,  Xvar), 

01splay_and_gr1nd(r2). 
ratvars:  rats. 

R2)$ 

Solve_tast():«  /•  Test  the  Solution  Routines  •/ 

block([], 

grlnd(solve.test) , 
grind: false, 

Solve„for^angle(C*l,  Phi), 

Sol*e_for_R(CSl .  A). 

/•  SOlve  a  type  (a)  surface  for  PSI  and  Y  •/ 

AS1:  S1nCos_to_CS{type_A_CSurfa».nri3), 

Sotva_for_angle(ASl.  PsI), 

Solva_for_A(ASl .  y). 

/*  SOlva  t  type  (8)  surface  for  PSI  and  Y  »/ 
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BS1 :  SinCos_to„CS( type_6„CSurf ace[ 13) . 

So1ve_for_ang1e(BSl ,  Psi). 

Sol va_for_X(6Sl .  y). 

■ot1fy() 

)S 

/*  CEnpress  tiprtittt  things  Canonically  */ 

CEnpreis(Exp,  Type)  :• 

Block([], 

If  Type  •  C_Surfate  than 
(Print(’C*nonicil  Linear  form 

Canon ical_l  inear_form(Exp)). 

Pr  int( "Sol ving  for  Angles...*}, 
for  Var  in  Angles  do 

(Solve_for_ang1e(£*p,  Var ) ) } S 

/*  Hera  at  Oo  the  Expression.  Now  to  get  Ground  forms,  just  change  GRIND,  ate.  •/ 
DO_CExpre*S():« 

BLOCk([], 

Grind:  Falsa.  /•  Hare  It  is..  •/ 

klll(labels), 

CExpress{type_A_Csurfaca[i3,  ‘C^Surfaca) , 

CExpress(type.JLCsurface[l].  *C_Surfaca) . 

CExpress(type_C^Csurfaca[l].  ’C_Surfaee) , 

CExpress(type_A^Aclaus«[l],  ’ACF). 

CExpress(type_B_AClause[l],  ’ACF))S 
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/•  Mactyma  BR0802  (Bruce  B.  Donald). 

Sova  intersections  oT  C„surfacas  and  A_claui#f. 
tor  tha  bdof  movers  probloat.  */ 

Angles:  [theta.  PM,  Psi]$ 


/•  allow  toms  simplification  into  C  and  S  taros  */ 

S1nCos„to_CS  (eapl  :• 

Block([E], 

E:  tap. 

for  Var  in  Angles  Do 
I:  rat*ub*t(»[var],  ain(war), 

ratsub»t{c[varj.  cos(var),  E)), 

rat(E))J 

/•  parmit  the  invarsc  •/ 

CS_to_SinCos  (tap)  :• 

B1ock([E]J, 

Is  tap, 

for  Var  in  Anglos  Do 
E:  rat$ubst(8ln(var),  s[var], 

ratsubst(cos{ear),c[var],  C) ) , 


/•  Tha  Ttngant  Space  Manifold  •/ 

/•  DO  It  yourself,  paII  batch1cad([dcfabc,nac,  *usrd$*.  brd})  •/ 

/•  Short  form  functions  •/ 

a(var)  :<*  a[war]t 
c(var)  :•  c[var]S 

/•  Simplify  if  posslblt?  usa  tha  e/c[var}  form  though.  */ 

S1mp_t(Eap,  Var)  :■  ratsubtt(l.  s(var}*2  *  c(var)'2,  tap)!/ 
s1mp_3(Eap)  :•  *at( 

Simp_l(  Siop_l(  S1mp_I(Eap,  PM).  Thata).  Pai))S 

Ksnifold(»ar):»  t(var)*2  +  c(var)*2  •  IS 

Bu11d_man1fold()  :• 

»lock([], 

Man[thata] :  manifold(thata), 

Man[Phi]  :  Mcnlfoie(pM) , 

Man[Pt1 j  :  Msnifold(psi))S 

BulldjnanifoldOS 

/*  E11mlnata[l]  allmlnatts  tha  dual  trig  tarn.  S'l  is  paramattrisad  by 
ont  variable  (war).  Eliminates  SIM(VAR)  from  ESP  •/ 

tl iminatati](aap,  war)  :■ 
block{[]. 

Tempi:  rat(part( 

tl1minata([a*p,  Nan[varl],  [s(var)]) .1)) 

)* 

/*  Eliminate^]  call*  elim1nata[l]  twice,  and  eliminates  the  resultants. 
Hence  the  intersection  of  two  level  surfaces  on  S* 3  it 
pemneteriied  by  a  ona  param.  family.  Eliminatas  Sln/cos  Varl/Var2 
from  Eap.  •/ 

USE_C5_fORM:  True* 

eliminate[2]{e*pl .  eap?,  warl.  war2)  :• 


/*  Temp  rtluHt  •/ 
/•  Tnelr  lacel*  •/ 


tlOCk(  £R1 .  *2.  *3,  R4,  R6 ,  r«. 

in.  Ir2 .  tr3.  Lr4.  tr6.  1r«]. 

RatVart:  [*.  'j .  x, 

»(phi),  c(phl).  <(  theta),  c(thata), 
c(p*1 )  ], 

if  Use_CS_FORH  than 

(e*pl:  SinCoa_TO_CS(**pl ) , 

.  e»pZ :  SinCos_To_CS(e»p2)). 

pr  1nt( "El  imnate  *,a(vai  J)."  from  enpretalon  1:*). 
rl:  el ininate[l](expl,  varl), 

Lrl: Idlsp(rl) , 

pMnt(“£i ininata  *,*(verl) from  e*pre»Hon  2:"). 
r2:  eUn1nat«[l](enp2 ,  varl), 

Lr2 :ldiip(r2) . 

pMnt("£l  imlnate  ’.c(varl).“  from  " . append(LRl ,  LK2)), 
r3:  rat(el1minate([rl  ,r2].[c(varl)])), 
lr3:  Ldisp(R3), 

print(’El  Icnlnate  *.»(var2).“  fro*  ”,lr3), 

R4:  el  iminate[l](R3.  var2), 

Lr4:  Lcnsp(M). 

Pr1nt("  Solve  ”,  Tr4,*  far  *.  c(vcr2)}, 

Rb:  rat{Solve(R4.  c(varZ))). 

Lr6 :  Lditp(R6). 

pMntl"Flnal1y,  aolve  *.Lrl.*  for  *.e(vari)). 

R6:  rat(«olve(Rl,  c(varl))), 

Lr6:  ldi*p(R6). 

Rppend{lr2,lr2.1r3.1r4.1r5,lre))J 


T**t():- 

#1 iroinate[2](type_4_Aclau*e[l), 
type_*_Aclau«e[2], 


/•  Moat:  Mtcsyaa 

(Ml*  to  Ron  Production  of  3d  Space  aquation*. 

Bruca  R.  Donald.  MIT  A1  LAB  )  •/ 

/*  Define  the  type  ABC  constraints.  •/ 

Oef1ne_ABC():- 

b1ock([]. 

.  batch ("USRDJ:[brd.prod)C space .wac"))$ 

/•  Define  the  appll Icablllty  Constraints  •/ 

Oaf1n*_Appl 1c{) :• 

■1ock(n. 

Batch  ( “USRDJ :  [brd . prod]Appl 1c.mac*))S 

/•  Define  Both  */ 

Producc_Osfs{):» 

b1ock(£], 

Wr1tefn*("usrdS:[brd.prod]Produee.log"), 

Batch("usrd$:[brd.prod]Rotate.#ac*). 

Def 1ne_A6C( ), 

Def1ne.App1Ic(). 

Closef iie(), 

Mot1fy())S 

Cold_Restart():« 

block([], 

batchloadf  "*ysSlog1n:utn*.i»ac") . 
batch1oad(”usrd$:£brd.prod]produce.i»ac"). 

Produce_defs())S 

/*  Hare’s  e  function  to  save  labels  for  you.  •/ 

Save_1abela(f11e):« 

block([], 

?Open_output_f lle(flle). 

for  Label  in  Appond(  reversa(1abels(e)},  reverte(lebe1s(d))) 

DO 

(T6r1nd_TO„FILf (Label ) ,  Pr1nt(Label )) . 

TCloseJ)utput_f11e())S 

/•  Produce  the  EXPRESS  file,  parsing  Into  solutions  and  coefficients.  •/ 

Produce_E»press():» 

block([], 

arrltef  11e("usrdS:  [brd.  prodjEaprsss.log"), 
batch  ( :  rnrct .  nrodlEaoress  .nac"). 

D0_CE*prs*s( ) , 

Prf nt("  Saving  Label*  In  LSP  file...'). 
Seve«Leb*lsCUSROS:[brd.prod]EXPRESS.lSP"). 

Mot1fy())S 

/•  this  function  produces  EVERYTHING.  •/ 

Produc*_ALL{):» 

■L0CK([], 

Produce_Oefs(). 

AU„0EFS_L0A0ED:  TRUE, 

K111(Nf 1,  Ng j ,  A1.  Alena.  Bj.  BJona), 

Type_A_C*urface[l)  :  A_6, 

Type_B_Csurf ac*[ 1] :  N_6. 

Type^C^Csurfacafl):  C„6. 

Produce_e*press(), 
closaf Ile())S 
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/*  Rruc*  R.  Donald  (BR090Z)  Euler  Rotations  for  Macsyma.  See  MACSYMA 
Paul,  p  46  •/ 

c(anglt):*  eo»(angla)S 
a(angla):*  s1n{ang1e)5 

«ot_*_ps<: 

aiptr1a( 

[  c(psl).  -a(psl).  0.  0]. 

(  a(p*1).  c { ps 1 ) ,  0.  0]. 

[  0‘.  0.  1.  0]. 

t  0.  0.  0.  l]j. 

Rot_y_theta:  matrlxj 
[  c(thsta).  0.  s(theta).  0], 

C  0.  1.  0.  0]. 

[-s(thati).  0,  c(thtta).  0], 

t  o.  0.  0.  1]): 

Rot_Z_Ph1:  Matr1x( 

[  c(ohi).  -s(pM),  0.  0]. 

[  S(pM).  c(ph1 ) .  0.  0]. 

[  0.  0.  1.  0], 

C  0.  0.  0.  1]).- 

Eular_tamp:  Rot_Y_Th*ta  .  Rot„Z_P*1; 

Eul*r.juatr1x:  Rot_Z_Ph1  .  Eular_tanp; 

homogen1i*{X):»  [x[l],  x[2J,  «[3],  S]; 

UnHornoganlza(X)  :•  *[23'*[4].  *C3]/*[4]].- 

Botate..veetor(X)  :•  UnHoniog*n1ze(  Euler_M*tr1x  .  Momoganise(X)) 

/■  Croc*  Product  */ 

Croit(A.B) 

C  (•[2]*b[3]  -  «C3]*b[2]). 

(a[3]*b[l]  -  a[l)-0[3)}. 

{•C33-6C2]  '  •[2]-bEi])].' 

/ 

/•  SInpl If jr  If  possible?  •/ 

S1np_l(Exp.  V*r)  :•  retsub»t(l,  *1n(v*r)*2  ♦  cos(v*r)*l,  oxp); 

*l«p_3(£*p)  :■  Rat( 

Si«p_l(  Slwp_l(  S1*p_l(Exp,  Phi).  That*),  Pal)); 

/*  General  Transformation  function.  A  4-vactor  Is  assumed  to  b*  a  plana, 
and  a  3-vactor  a  3-vactor.  hancs  tha  3-ractor  1»  ROTATED  and  a  4'vector 

nlona  4 «  klcn  pntatari  Ca»  OIIII  •/ 

r  •  — —  *  —  ■  ^  .  **■  ’•”»  • 

Transform(Transfonoat1on_jnatr1x,  X) 

01 ock( [Horn,  Trans. ant], 

Ans:  'Whoopsl*, 

If  langth(X)  *  4  than 

Ans:  a  .Transformat1on_matr1* 

CIS* 

If  langth  (x)  •  3  than 

Ans :Unhomog»n1z*(Tr»nsformat1on_mntr1x  .  Homogenize  (X)) 

Els*  Print("  But  Must  be  a  S-vactor  or  4-Vactorl'), 

Ana); 


/*  Now  Compute  Invars*  of  the  Euler  Transformation  */ 
E_Adj:  *1mp_3(r*t  (idjo1nt(  Eul*r_«atr1a) ) ) ; 
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